From 487918655d6d79efae8dfbcbf15fd442686621aa Mon Sep 17 00:00:00 2001 From: gavanderhoorn Date: Mon, 6 Apr 2020 16:31:39 +0200 Subject: [PATCH] Remove abb_driver. Migrated to 'ros-industrial/abb_driver'. Ref: ros-industrial/abb#179. --- abb_driver/CHANGELOG.rst | 70 ----- abb_driver/CMakeLists.txt | 52 ---- abb_driver/include/abb_driver/abb_utils.h | 66 ----- abb_driver/launch/robot_interface.launch | 38 --- abb_driver/package.xml | 28 -- abb_driver/rapid/ROS_common.sys | 45 --- abb_driver/rapid/ROS_messages.sys | 271 ------------------- abb_driver/rapid/ROS_motion.mod | 126 --------- abb_driver/rapid/ROS_motionServer.mod | 119 -------- abb_driver/rapid/ROS_socket.sys | 104 ------- abb_driver/rapid/ROS_stateServer.mod | 163 ----------- abb_driver/src/abb_joint_downloader_node.cpp | 86 ------ abb_driver/src/abb_robot_state_node.cpp | 81 ------ abb_driver/src/abb_utils.cpp | 57 ---- 14 files changed, 1306 deletions(-) delete mode 100644 abb_driver/CHANGELOG.rst delete mode 100644 abb_driver/CMakeLists.txt delete mode 100644 abb_driver/include/abb_driver/abb_utils.h delete mode 100644 abb_driver/launch/robot_interface.launch delete mode 100644 abb_driver/package.xml delete mode 100644 abb_driver/rapid/ROS_common.sys delete mode 100644 abb_driver/rapid/ROS_messages.sys delete mode 100644 abb_driver/rapid/ROS_motion.mod delete mode 100644 abb_driver/rapid/ROS_motionServer.mod delete mode 100644 abb_driver/rapid/ROS_socket.sys delete mode 100644 abb_driver/rapid/ROS_stateServer.mod delete mode 100644 abb_driver/src/abb_joint_downloader_node.cpp delete mode 100644 abb_driver/src/abb_robot_state_node.cpp delete mode 100644 abb_driver/src/abb_utils.cpp diff --git a/abb_driver/CHANGELOG.rst b/abb_driver/CHANGELOG.rst deleted file mode 100644 index 0346f577..00000000 --- a/abb_driver/CHANGELOG.rst +++ /dev/null @@ -1,70 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package abb_driver -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.3.1 (2019-09-17) ------------------- -* Add robot status publishing to Rapid driver (`#168 `_) -* Fix is_near check (`#155 `_) -* Add support for external linear axes to abb_driver (`#150 `_) -* Use the 'doc' attribute on 'arg' elements. (`#143 `_) -* Update maintainers (`#139 `_) -* Contributors: Gonzalo Casas, Harsh Deshpande, JD Yamokoski, Keerthana Manivannan, gavanderhoorn - -1.3.0 (2017-05-27) ------------------- -* kinetic-devel release of ros-industrial/abb -* Contributors: AustinDeric - -1.2.1 (2017-03-27) ------------------- -* No changes - -1.2.0 (2015-06-06) ------------------- -* No changes - -1.1.9 (2015-04-07) ------------------- -* No changes - -1.1.8 (2015-04-06) ------------------- -* No changes - -1.1.7 (2015-04-01) ------------------- -* Merged hydro branch - - Updated CHANGELOG.rst and package.xml files -* Contributors: Levi Armstrong - -1.1.6 (2015-03-17) ------------------- -* Fix typos and links in CHANGELOG.rst -* Contributors: Levi Armstrong - -1.1.5 (2015-03-17) ------------------- -* driver: ROS_motionServer.mod was checking for wrong comm type to send reply - message, causing driver to quit working after stop command. - Fix `#42 `_. -* Contributors: Levi Armstrong - -1.1.4 (2014-12-14) ------------------- -* No changes - -1.1.3 (2014-09-05) ------------------- -* driver: reintroduce coupling factor default. - Reverts 3765cd6. -* Bump versions. -* driver: remove default for J23 coupling parameter. - Users should explicitly provide this on the command line, or use one - of the convenience launchfiles provided with the support packages. - It is of critical importance that this parameter is set to the - correct value, and should therefore not be supplied a default in - a (for end-users) file with low visibility. -* driver: move driver (Rapid and nodes) into separate package. - Node sources, headers and launch files copied from abb_common. -* Contributors: gavanderhoorn diff --git a/abb_driver/CMakeLists.txt b/abb_driver/CMakeLists.txt deleted file mode 100644 index 0b612edb..00000000 --- a/abb_driver/CMakeLists.txt +++ /dev/null @@ -1,52 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(abb_driver) -add_definitions(-std=c++11) -find_package(catkin REQUIRED COMPONENTS industrial_robot_client simple_message) - -catkin_package( - CATKIN_DEPENDS industrial_robot_client -) - -add_definitions(-DLINUXSOCKETS=1) - -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - - -add_executable(${PROJECT_NAME}_robot_state - src/abb_robot_state_node.cpp - src/abb_utils.cpp) -target_link_libraries(${PROJECT_NAME}_robot_state - industrial_robot_client - simple_message - ${catkin_LIBRARIES}) -set_target_properties( - ${PROJECT_NAME}_robot_state - PROPERTIES OUTPUT_NAME robot_state PREFIX "") - -add_executable(${PROJECT_NAME}_motion_download_interface - src/abb_joint_downloader_node.cpp - src/abb_utils.cpp) -target_link_libraries(${PROJECT_NAME}_motion_download_interface - industrial_robot_client - simple_message - ${catkin_LIBRARIES}) -set_target_properties(${PROJECT_NAME}_motion_download_interface - PROPERTIES OUTPUT_NAME motion_download_interface 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() diff --git a/abb_driver/include/abb_driver/abb_utils.h b/abb_driver/include/abb_driver/abb_utils.h deleted file mode 100644 index 10fe2dab..00000000 --- a/abb_driver/include/abb_driver/abb_utils.h +++ /dev/null @@ -1,66 +0,0 @@ -/* - * 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 -#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& joints_in, std::vector* 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_ */ diff --git a/abb_driver/launch/robot_interface.launch b/abb_driver/launch/robot_interface.launch deleted file mode 100644 index 5613c103..00000000 --- a/abb_driver/launch/robot_interface.launch +++ /dev/null @@ -1,38 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/abb_driver/package.xml b/abb_driver/package.xml deleted file mode 100644 index 8db413a4..00000000 --- a/abb_driver/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - abb_driver - 1.3.1 - -

- ROS-Industrial nodes for interfacing with ABB robot controllers. -

-

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

- - Edward Venator - Jeremy Zoss - Shaun Edwards - Levi Armstrong (Southwest Research Institute) - BSD - - http://ros.org/wiki/abb_driver - https://github.com/ros-industrial/abb/issues - https://github.com/ros-industrial/abb - - catkin - - industrial_robot_client - - diff --git a/abb_driver/rapid/ROS_common.sys b/abb_driver/rapid/ROS_common.sys deleted file mode 100644 index 3bd048f6..00000000 --- a/abb_driver/rapid/ROS_common.sys +++ /dev/null @@ -1,45 +0,0 @@ -MODULE ROS_common(SYSMODULE) - -! Software License Agreement (BSD License) -! -! Copyright (c) 2012, Edward Venator, Case Western Reserve University -! Copyright (c) 2012, Jeremy Zoss, 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 Case Western Reserve University nor the names of its contributors -! may be used to endorse or promote products derived from this software without -! specific prior written permission. -! -! THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY -! EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES -! OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT -! SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -! INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED -! TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR -! BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -! CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -! WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -RECORD ROS_joint_trajectory_pt - robjoint joint_pos; - extjoint extax_pos; - num duration; -ENDRECORD - -CONST num MAX_TRAJ_LENGTH := 100; - -! These variables should use TestAndSet read/write protection to prevent conflicts -PERS bool ROS_trajectory_lock := false; -PERS ROS_joint_trajectory_pt ROS_trajectory{MAX_TRAJ_LENGTH}; -PERS num ROS_trajectory_size := 0; -PERS bool ROS_new_trajectory := false; ! can safely READ, but should use lock to WRITE - -ENDMODULE diff --git a/abb_driver/rapid/ROS_messages.sys b/abb_driver/rapid/ROS_messages.sys deleted file mode 100644 index 596262bf..00000000 --- a/abb_driver/rapid/ROS_messages.sys +++ /dev/null @@ -1,271 +0,0 @@ -MODULE ROS_messages(SYSMODULE) -! Software License Agreement (BSD License) -! -! Copyright (c) 2012, Jeremy Zoss, 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 Case Western Reserve University nor the names of its contributors -! may be used to endorse or promote products derived from this software without -! specific prior written permission. -! -! THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY -! EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES -! OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT -! SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -! INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED -! TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR -! BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -! CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -! WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -RECORD ROS_msg_header - num msg_type; - num comm_type; - num reply_code; -ENDRECORD - -RECORD ROS_msg - ROS_msg_header header; - rawbytes data; -ENDRECORD - -RECORD ROS_msg_joint_traj_pt - ROS_msg_header header; - num sequence_id; - robjoint joints; ! in DEGREES - extjoint ext_axes; - num velocity; - num duration; -ENDRECORD - -RECORD ROS_msg_joint_data - ROS_msg_header header; - num sequence_id; - robjoint joints; ! in DEGREES - extjoint ext_axes; -ENDRECORD - -RECORD ROS_msg_robot_status - ROS_msg_header header; - num sequence_id; - num drives_powered; - num e_stopped; - num error_code; - num in_error; - num in_motion; - num mode; - num motion_possible; -ENDRECORD - -! Message Type Codes (from simple_message/simple_message.h) -CONST num ROS_MSG_TYPE_INVALID := 0; -CONST num ROS_MSG_TYPE_JOINT := 10; ! joint-position feedback -CONST num ROS_MSG_TYPE_JOINT_TRAJ_PT := 11; ! joint-trajectory-point (for path downloading) -CONST num ROS_MSG_TYPE_STATUS := 13; ! robot status message (for reporting the robot state) -CONST num ROS_COM_TYPE_TOPIC := 1; -CONST num ROS_COM_TYPE_SRV_REQ := 2; -CONST num ROS_COM_TYPE_SRV_REPLY := 3; -CONST num ROS_REPLY_TYPE_INVALID := 0; -CONST num ROS_REPLY_TYPE_SUCCESS := 1; -CONST num ROS_REPLY_TYPE_FAILURE := 2; - -! "Special" Sequence Codes (from simple_message/joint_traj_pt.h) -CONST num ROS_TRAJECTORY_START_DOWNLOAD := -1; -CONST num ROS_TRAJECTORY_END := -3; -CONST num ROS_TRAJECTORY_STOP := -4; - -! Robot mode codes (from industrial_msgs/RobotMode.msg) -CONST num ROS_ROBOT_MODE_UNKNOWN := -1; ! Unknown or unavailable -CONST num ROS_ROBOT_MODE_MANUAL := 1; ! Teach OR manual mode -CONST num ROS_ROBOT_MODE_AUTO := 2; ! Automatic mode - -! Tri-state values (from inudstrial/TriState.msg) -CONST num ROS_TRISTATE_UNKNOWN := -1; -CONST num ROS_TRISTATE_TRUE := 1; -CONST num ROS_TRISTATE_ON := 1; -CONST num ROS_TRISTATE_FALSE := 0; -CONST num ROS_TRISTATE_OFF := 0; - -! Other message constants -CONST num ROS_MSG_MAX_JOINTS := 10; ! from joint_data.h - -PROC ROS_receive_msg_joint_traj_pt(VAR socketdev client_socket, VAR ROS_msg_joint_traj_pt message, \num wait_time) - VAR ROS_msg raw_message; - - ! Read raw message data - IF Present(wait_time) THEN - ROS_receive_msg client_socket, raw_message, \wait_time:=wait_time; - ELSE - ROS_receive_msg client_socket, raw_message; - ENDIF - - ! Integrity Check: Message Type - IF (raw_message.header.msg_type <> ROS_MSG_TYPE_JOINT_TRAJ_PT) THEN - ErrWrite \W, "ROS Socket Type Mismatch", "Unexpected message type", - \RL2:="expected: " + ValToStr(ROS_MSG_TYPE_JOINT_TRAJ_PT), - \RL3:="received: " + ValToStr(raw_message.header.msg_type); - RAISE ERR_ARGVALERR; ! TBD: define specific error code - ENDIF - - ! Integrity Check: Data Size - IF (RawBytesLen(raw_message.data) < 52) THEN - ErrWrite \W, "ROS Socket Missing Data", "Insufficient data for joint_trajectory_pt", - \RL2:="expected: 52", - \RL3:="received: " + ValToStr(RawBytesLen(raw_message.data)); - RAISE ERR_OUTOFBND; ! TBD: define specific error code - ENDIF - - ! Copy Header data - message.header := raw_message.header; - - ! Unpack data fields - UnpackRawBytes raw_message.data, 1, message.sequence_id, \IntX:=DINT; - UnpackRawBytes raw_message.data, 5, message.joints.rax_1, \Float4; - UnpackRawBytes raw_message.data, 9, message.joints.rax_2, \Float4; - UnpackRawBytes raw_message.data, 13, message.joints.rax_3, \Float4; - UnpackRawBytes raw_message.data, 17, message.joints.rax_4, \Float4; - UnpackRawBytes raw_message.data, 21, message.joints.rax_5, \Float4; - UnpackRawBytes raw_message.data, 25, message.joints.rax_6, \Float4; - UnpackRawBytes raw_message.data, 29, message.ext_axes.eax_a, \Float4; - UnpackRawBytes raw_message.data, 33, message.ext_axes.eax_b, \Float4; - UnpackRawBytes raw_message.data, 37, message.ext_axes.eax_c, \Float4; - UnpackRawBytes raw_message.data, 41, message.ext_axes.eax_d, \Float4; - UnpackRawBytes raw_message.data, 45, message.velocity, \Float4; - UnpackRawBytes raw_message.data, 49, message.duration, \Float4; - - ! Convert data from ROS units to ABB units - message.joints := rad2deg_robjoint(message.joints); - message.ext_axes := m2mm_extjoint(message.ext_axes); - ! TBD: convert velocity - -ERROR - RAISE; ! raise errors to calling code -ENDPROC - -PROC ROS_send_msg_joint_data(VAR socketdev client_socket, ROS_msg_joint_data message) - VAR ROS_msg raw_message; - VAR robjoint ROS_joints; - VAR extjoint ROS_ext_axes; - VAR num i; - - ! Force message header to the correct values - raw_message.header.msg_type := ROS_MSG_TYPE_JOINT; - raw_message.header.comm_type := ROS_COM_TYPE_TOPIC; - raw_message.header.reply_code := ROS_REPLY_TYPE_INVALID; - - ! Convert data from ABB units to ROS units - ROS_joints := deg2rad_robjoint(message.joints); - ROS_ext_axes := mm2m_extjoint(message.ext_axes); - - ! Pack data into message - PackRawBytes message.sequence_id, raw_message.data, 1, \IntX:=DINT; - PackRawBytes ROS_joints.rax_1, raw_message.data, 5, \Float4; - PackRawBytes ROS_joints.rax_2, raw_message.data, 9, \Float4; - PackRawBytes ROS_joints.rax_3, raw_message.data, 13, \Float4; - PackRawBytes ROS_joints.rax_4, raw_message.data, 17, \Float4; - PackRawBytes ROS_joints.rax_5, raw_message.data, 21, \Float4; - PackRawBytes ROS_joints.rax_6, raw_message.data, 25, \Float4; - PackRawBytes ROS_ext_axes.eax_a, raw_message.data, 29, \Float4; - PackRawBytes ROS_ext_axes.eax_b, raw_message.data, 33, \Float4; - PackRawBytes ROS_ext_axes.eax_c, raw_message.data, 37, \Float4; - PackRawBytes ROS_ext_axes.eax_d, raw_message.data, 41, \Float4; - - ROS_send_msg client_socket, raw_message; - -ERROR - RAISE; ! raise errors to calling code -ENDPROC - -PROC ROS_send_msg_robot_status(VAR socketdev client_socket, ROS_msg_robot_status message) - VAR ROS_msg raw_message; - - ! Force message header to the correct values - raw_message.header.msg_type := message.header.msg_type; - raw_message.header.comm_type := message.header.comm_type; - raw_message.header.reply_code := message.header.reply_code; - - ! Pack data into message - PackRawBytes message.drives_powered, raw_message.data, 1, \IntX:=DINT; - PackRawBytes message.e_stopped, raw_message.data, 5, \IntX:=DINT; - PackRawBytes message.error_code, raw_message.data, 9, \IntX:=DINT; - PackRawBytes message.in_error, raw_message.data, 13, \IntX:=DINT; - PackRawBytes message.in_motion, raw_message.data, 17, \IntX:=DINT; - PackRawBytes message.mode, raw_message.data, 21, \IntX:=DINT; - PackRawBytes message.motion_possible, raw_message.data, 25, \IntX:=DINT; - - ROS_send_msg client_socket, raw_message; - -ERROR - RAISE; ! raise errors to calling code -ENDPROC - -LOCAL FUNC num deg2rad(num deg) - RETURN deg * pi / 180; -ENDFUNC - -LOCAL FUNC num connected_eax(num eax) - ! The value 9E9 is defined for axes which are not connected - IF eax <> 9E9 THEN - RETURN eax; - ENDIF - RETURN 0; -ENDFUNC - -! Returns only connected external axes in METERS -LOCAL FUNC extjoint mm2m_extjoint(extjoint all_eax) - VAR extjoint eax; - eax.eax_a := connected_eax(all_eax.eax_a) / 1000; - eax.eax_b := connected_eax(all_eax.eax_b) / 1000; - eax.eax_c := connected_eax(all_eax.eax_c) / 1000; - eax.eax_d := connected_eax(all_eax.eax_d) / 1000; - RETURN eax; -ENDFUNC - -! Returns external axes in MILLIMETERS -LOCAL FUNC extjoint m2mm_extjoint(extjoint eax_in_m) - VAR extjoint eax_in_mm; - eax_in_mm.eax_a := eax_in_m.eax_a * 1000; - eax_in_mm.eax_b := eax_in_m.eax_b * 1000; - eax_in_mm.eax_c := eax_in_m.eax_c * 1000; - eax_in_mm.eax_d := eax_in_m.eax_d * 1000; - - RETURN eax_in_mm; -ENDFUNC - -LOCAL FUNC robjoint deg2rad_robjoint(robjoint deg) - VAR robjoint rad; - rad.rax_1 := deg2rad(deg.rax_1); - rad.rax_2 := deg2rad(deg.rax_2); - rad.rax_3 := deg2rad(deg.rax_3); - rad.rax_4 := deg2rad(deg.rax_4); - rad.rax_5 := deg2rad(deg.rax_5); - rad.rax_6 := deg2rad(deg.rax_6); - - RETURN rad; -ENDFUNC - -LOCAL FUNC num rad2deg(num rad) - RETURN rad * 180 / pi; -ENDFUNC - -LOCAL FUNC robjoint rad2deg_robjoint(robjoint rad) - VAR robjoint deg; - deg.rax_1 := rad2deg(rad.rax_1); - deg.rax_2 := rad2deg(rad.rax_2); - deg.rax_3 := rad2deg(rad.rax_3); - deg.rax_4 := rad2deg(rad.rax_4); - deg.rax_5 := rad2deg(rad.rax_5); - deg.rax_6 := rad2deg(rad.rax_6); - - RETURN deg; -ENDFUNC - -ENDMODULE diff --git a/abb_driver/rapid/ROS_motion.mod b/abb_driver/rapid/ROS_motion.mod deleted file mode 100644 index 5a1f65de..00000000 --- a/abb_driver/rapid/ROS_motion.mod +++ /dev/null @@ -1,126 +0,0 @@ -MODULE ROS_motion - -! Software License Agreement (BSD License) -! -! Copyright (c) 2012, Edward Venator, Case Western Reserve University -! Copyright (c) 2012, Jeremy Zoss, 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 Case Western Reserve University nor the names of its contributors -! may be used to endorse or promote products derived from this software without -! specific prior written permission. -! -! THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY -! EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES -! OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT -! SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -! INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED -! TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR -! BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -! CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -! WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -LOCAL CONST zonedata DEFAULT_CORNER_DIST := z10; -LOCAL VAR ROS_joint_trajectory_pt trajectory{MAX_TRAJ_LENGTH}; -LOCAL VAR num trajectory_size := 0; -LOCAL VAR intnum intr_new_trajectory; - -PROC main() - VAR num current_index; - VAR jointtarget target; - VAR speeddata move_speed := v10; ! default speed - VAR zonedata stop_mode; - VAR bool skip_move; - - ! Set up interrupt to watch for new trajectory - IDelete intr_new_trajectory; ! clear interrupt handler, in case restarted with ExitCycle - CONNECT intr_new_trajectory WITH new_trajectory_handler; - IPers ROS_new_trajectory, intr_new_trajectory; - - WHILE true DO - ! Check for new Trajectory - IF (ROS_new_trajectory) - init_trajectory; - - ! execute all points in this trajectory - IF (trajectory_size > 0) THEN - FOR current_index FROM 1 TO trajectory_size DO - target.robax := trajectory{current_index}.joint_pos; - target.extax := trajectory{current_index}.extax_pos; - - skip_move := (current_index = 1) AND is_near(target, 0.1, 0.1); - - stop_mode := DEFAULT_CORNER_DIST; ! assume we're smoothing between points - IF (current_index = trajectory_size) stop_mode := fine; ! stop at path end - - ! Execute move command - IF (NOT skip_move) - MoveAbsJ target, move_speed, \T:=trajectory{current_index}.duration, stop_mode, tool0; - ENDFOR - - trajectory_size := 0; ! trajectory done - ENDIF - - WaitTime 0.05; ! Throttle loop while waiting for new command - ENDWHILE -ERROR - ErrWrite \W, "Motion Error", "Error executing motion. Aborting trajectory."; - abort_trajectory; -ENDPROC - -LOCAL PROC init_trajectory() - clear_path; ! cancel any active motions - - WaitTestAndSet ROS_trajectory_lock; ! acquire data-lock - trajectory := ROS_trajectory; ! copy to local var - trajectory_size := ROS_trajectory_size; ! copy to local var - ROS_new_trajectory := FALSE; - ROS_trajectory_lock := FALSE; ! release data-lock -ENDPROC - -LOCAL FUNC bool is_near(jointtarget target, num deg_tol, num mm_tol) - VAR jointtarget curr_jnt; - - curr_jnt := CJointT(); - - ! either an external axis is unconfigured/not present OR if it is, then it must be close enough - RETURN ( ABS(curr_jnt.robax.rax_1 - target.robax.rax_1) < deg_tol ) - AND ( ABS(curr_jnt.robax.rax_2 - target.robax.rax_2) < deg_tol ) - AND ( ABS(curr_jnt.robax.rax_3 - target.robax.rax_3) < deg_tol ) - AND ( ABS(curr_jnt.robax.rax_4 - target.robax.rax_4) < deg_tol ) - AND ( ABS(curr_jnt.robax.rax_5 - target.robax.rax_5) < deg_tol ) - AND ( ABS(curr_jnt.robax.rax_6 - target.robax.rax_6) < deg_tol ) - AND ( (curr_jnt.extax.eax_a = 9E9) OR (ABS(curr_jnt.extax.eax_a - target.extax.eax_a) < mm_tol) ) - AND ( (curr_jnt.extax.eax_b = 9E9) OR (ABS(curr_jnt.extax.eax_b - target.extax.eax_b) < mm_tol) ) - AND ( (curr_jnt.extax.eax_c = 9E9) OR (ABS(curr_jnt.extax.eax_c - target.extax.eax_c) < mm_tol) ) - AND ( (curr_jnt.extax.eax_d = 9E9) OR (ABS(curr_jnt.extax.eax_d - target.extax.eax_d) < mm_tol) ); -ENDFUNC - -LOCAL PROC abort_trajectory() - trajectory_size := 0; ! "clear" local trajectory - clear_path; - ExitCycle; ! restart program -ENDPROC - -LOCAL PROC clear_path() - IF ( NOT (IsStopMoveAct(\FromMoveTask) OR IsStopMoveAct(\FromNonMoveTask)) ) - StopMove; ! stop any active motions - ClearPath; ! clear queued motion commands - StartMove; ! re-enable motions -ENDPROC - -LOCAL TRAP new_trajectory_handler - IF (NOT ROS_new_trajectory) RETURN; - - abort_trajectory; -ENDTRAP - -ENDMODULE diff --git a/abb_driver/rapid/ROS_motionServer.mod b/abb_driver/rapid/ROS_motionServer.mod deleted file mode 100644 index 88abc817..00000000 --- a/abb_driver/rapid/ROS_motionServer.mod +++ /dev/null @@ -1,119 +0,0 @@ -MODULE ROS_motionServer - -! Software License Agreement (BSD License) -! -! Copyright (c) 2012, Edward Venator, Case Western Reserve University -! Copyright (c) 2012, Jeremy Zoss, 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 Case Western Reserve University nor the names of its contributors -! may be used to endorse or promote products derived from this software without -! specific prior written permission. -! -! THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY -! EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES -! OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT -! SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -! INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED -! TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR -! BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -! CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -! WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -LOCAL CONST num server_port := 11000; - -LOCAL VAR socketdev server_socket; -LOCAL VAR socketdev client_socket; -LOCAL VAR ROS_joint_trajectory_pt trajectory{MAX_TRAJ_LENGTH}; -LOCAL VAR num trajectory_size; - -PROC main() - VAR ROS_msg_joint_traj_pt message; - - TPWrite "MotionServer: Waiting for connection."; - ROS_init_socket server_socket, server_port; - ROS_wait_for_client server_socket, client_socket; - - WHILE ( true ) DO - ! Recieve Joint Trajectory Pt Message - ROS_receive_msg_joint_traj_pt client_socket, message; - trajectory_pt_callback message; - ENDWHILE - -ERROR (ERR_SOCK_TIMEOUT, ERR_SOCK_CLOSED) - IF (ERRNO=ERR_SOCK_TIMEOUT) OR (ERRNO=ERR_SOCK_CLOSED) THEN - SkipWarn; ! TBD: include this error data in the message logged below? - ErrWrite \W, "ROS MotionServer disconnect", "Connection lost. Resetting socket."; - ExitCycle; ! restart program - ELSE - TRYNEXT; - ENDIF -UNDO - IF (SocketGetStatus(client_socket) <> SOCKET_CLOSED) SocketClose client_socket; - IF (SocketGetStatus(server_socket) <> SOCKET_CLOSED) SocketClose server_socket; -ENDPROC - -LOCAL PROC trajectory_pt_callback(ROS_msg_joint_traj_pt message) - VAR ROS_joint_trajectory_pt point; - VAR jointtarget current_pos; - VAR ROS_msg reply_msg; - - point := [message.joints, message.ext_axes, message.duration]; - - ! use sequence_id to signal start/end of trajectory download - TEST message.sequence_id - CASE ROS_TRAJECTORY_START_DOWNLOAD: - TPWrite "Traj START received"; - trajectory_size := 0; ! Reset trajectory size - add_traj_pt point; ! Add this point to the trajectory - CASE ROS_TRAJECTORY_END: - TPWrite "Traj END received"; - add_traj_pt point; ! Add this point to the trajectory - activate_trajectory; - CASE ROS_TRAJECTORY_STOP: - TPWrite "Traj STOP received"; - trajectory_size := 0; ! empty trajectory - activate_trajectory; - StopMove; ClearPath; StartMove; ! redundant, but re-issue stop command just to be safe - DEFAULT: - add_traj_pt point; ! Add this point to the trajectory - ENDTEST - - ! send reply, if requested - IF (message.header.comm_type = ROS_COM_TYPE_SRV_REQ) THEN - reply_msg.header := [ROS_MSG_TYPE_JOINT_TRAJ_PT, ROS_COM_TYPE_SRV_REPLY, ROS_REPLY_TYPE_SUCCESS]; - ROS_send_msg client_socket, reply_msg; - ENDIF - -ERROR - RAISE; ! raise errors to calling code -ENDPROC - -LOCAL PROC add_traj_pt(ROS_joint_trajectory_pt point) - IF (trajectory_size = MAX_TRAJ_LENGTH) THEN - ErrWrite \W, "Too Many Trajectory Points", "Trajectory has already reached its maximum size", - \RL2:="max_size = " + ValToStr(MAX_TRAJ_LENGTH); - ELSE - Incr trajectory_size; - trajectory{trajectory_size} := point; !Add this point to the trajectory - ENDIF -ENDPROC - -LOCAL PROC activate_trajectory() - WaitTestAndSet ROS_trajectory_lock; ! acquire data-lock - TPWrite "Sending " + ValToStr(trajectory_size) + " points to MOTION task"; - ROS_trajectory := trajectory; - ROS_trajectory_size := trajectory_size; - ROS_new_trajectory := TRUE; - ROS_trajectory_lock := FALSE; ! release data-lock -ENDPROC - -ENDMODULE diff --git a/abb_driver/rapid/ROS_socket.sys b/abb_driver/rapid/ROS_socket.sys deleted file mode 100644 index 86a8aaea..00000000 --- a/abb_driver/rapid/ROS_socket.sys +++ /dev/null @@ -1,104 +0,0 @@ -MODULE ROS_socket(SYSMODULE) -! Software License Agreement (BSD License) -! -! Copyright (c) 2012, Jeremy Zoss, Southwest Research Institute -! Copyright (c) 2012, Edward Venator, Case Western Reserve University -! 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 Case Western Reserve University nor the names of its contributors -! may be used to endorse or promote products derived from this software without -! specific prior written permission. -! -! THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY -! EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES -! OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT -! SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -! INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED -! TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR -! BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -! CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -! WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -PROC ROS_init_socket(VAR socketdev server_socket, num port) - IF (SocketGetStatus(server_socket) = SOCKET_CLOSED) SocketCreate server_socket; - IF (SocketGetStatus(server_socket) = SOCKET_CREATED) SocketBind server_socket, GetSysInfo(\LanIp), port; - IF (SocketGetStatus(server_socket) = SOCKET_BOUND) SocketListen server_socket; - -ERROR - RAISE; ! raise errors to calling code -ENDPROC - -PROC ROS_wait_for_client(VAR socketdev server_socket, VAR socketdev client_socket, \num wait_time) - VAR string client_ip; - VAR num time_val := WAIT_MAX; ! default to wait-forever - - IF Present(wait_time) time_val := wait_time; - - IF (SocketGetStatus(client_socket) <> SOCKET_CLOSED) SocketClose client_socket; - WaitUntil (SocketGetStatus(client_socket) = SOCKET_CLOSED); - - SocketAccept server_socket, client_socket, \ClientAddress:=client_ip, \Time:=time_val; - TPWrite "Client at "+client_ip+" connected."; - -ERROR - RAISE; ! raise errors to calling code -ENDPROC - -PROC ROS_receive_msg(VAR socketdev client_socket, VAR ROS_msg message, \num wait_time) - VAR rawbytes buffer; - VAR num time_val := WAIT_MAX; ! default to wait-forever - VAR num bytes_rcvd; - VAR num msg_length; - - ClearRawBytes buffer; - IF Present(wait_time) time_val := wait_time; - - ! TBD: need to determine whether this handles split/merged messages correctly - - ! Read prefix INT (total message length) - SocketReceive client_socket, \RawData:=buffer, \ReadNoOfBytes:=4, \Time:=time_val; - UnpackRawBytes buffer, 1, msg_length, \IntX:=UDINT; - - ! Read remaining message bytes - SocketReceive client_socket, \RawData:=buffer, \ReadNoOfBytes:=msg_length, \NoRecBytes:=bytes_rcvd, \Time:=time_val; - IF (bytes_rcvd <> msg_length) THEN - ErrWrite \W, "ROS Socket Recv Failed", "Did not receive expected # of bytes.", - \RL2:="Expected: " + ValToStr(msg_length), - \RL3:="Received: " + ValToStr(bytes_rcvd); - RETURN; - ENDIF - - ! Unpack message header/data - UnpackRawBytes buffer, 1, message.header.msg_type, \IntX:=DINT; - UnpackRawBytes buffer, 5, message.header.comm_type, \IntX:=DINT; - UnpackRawBytes buffer, 9, message.header.reply_code, \IntX:=DINT; - CopyRawBytes buffer, 13, message.data, 1; - -ERROR - RAISE; ! raise errors to calling code -ENDPROC - -PROC ROS_send_msg(VAR socketdev client_socket, VAR ROS_msg message) - VAR rawbytes buffer; - - PackRawBytes 12 + RawBytesLen(message.data), buffer, 1, \IntX := UDINT; ! Packet length (excluding this prefix) - PackRawBytes message.header.msg_type, buffer, 5, \IntX := DINT; ! Message type - PackRawBytes message.header.comm_type, buffer, 9, \IntX := DINT; ! Comm type - PackRawBytes message.header.reply_code, buffer, 13, \IntX := DINT; ! Reply code - CopyRawBytes message.data, 1, buffer, 17; ! Message data - - SocketSend client_socket \RawData:=buffer; - -ERROR - RAISE; ! raise errors to calling code -ENDPROC - -ENDMODULE diff --git a/abb_driver/rapid/ROS_stateServer.mod b/abb_driver/rapid/ROS_stateServer.mod deleted file mode 100644 index 0b58ba68..00000000 --- a/abb_driver/rapid/ROS_stateServer.mod +++ /dev/null @@ -1,163 +0,0 @@ -MODULE ROS_stateServer - -! Software License Agreement (BSD License) -! -! Copyright (c) 2012, Edward Venator, Case Western Reserve University -! Copyright (c) 2012, Jeremy Zoss, 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 Case Western Reserve University nor the names of its contributors -! may be used to endorse or promote products derived from this software without -! specific prior written permission. -! -! THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY -! EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES -! OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT -! SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -! INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED -! TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR -! BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -! CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -! WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -LOCAL CONST num server_port := 11002; -LOCAL CONST num update_rate := 0.10; ! broadcast rate (sec) - -LOCAL VAR socketdev server_socket; -LOCAL VAR socketdev client_socket; - -PROC main() - - TPWrite "StateServer: Waiting for connection."; - ROS_init_socket server_socket, server_port; - ROS_wait_for_client server_socket, client_socket; - - WHILE (TRUE) DO - send_joints; - send_status; - WaitTime update_rate; - ENDWHILE - -ERROR (ERR_SOCK_TIMEOUT, ERR_SOCK_CLOSED) - IF (ERRNO=ERR_SOCK_TIMEOUT) OR (ERRNO=ERR_SOCK_CLOSED) THEN - SkipWarn; ! TBD: include this error data in the message logged below? - ErrWrite \W, "ROS StateServer disconnect", "Connection lost. Waiting for new connection."; - ExitCycle; ! restart program - ELSE - TRYNEXT; - ENDIF -UNDO -ENDPROC - -LOCAL PROC send_joints() - VAR ROS_msg_joint_data message; - VAR jointtarget joints; - - ! get current joint position (degrees) - joints := CJointT(); - - ! create message - message.header := [ROS_MSG_TYPE_JOINT, ROS_COM_TYPE_TOPIC, ROS_REPLY_TYPE_INVALID]; - message.sequence_id := 0; - message.joints := joints.robax; - message.ext_axes := joints.extax; - - ! send message to client - ROS_send_msg_joint_data client_socket, message; - -ERROR - RAISE; ! raise errors to calling code -ENDPROC - -! signalExecutionError : System Output -! signalMotionPossible : System Output -! signalMotorOn : System Output -! signalRobotActive : System Output -! signalRobotEStop : System Output -! signalRobotNotMoving : System Output -! signalRosMotionTaskExecuting : System Output -LOCAL PROC send_status() - VAR ROS_msg_robot_status message; - - ! get current joint position (degrees) - ! joints := CJointT(); - - ! create message - message.header := [ROS_MSG_TYPE_STATUS, ROS_COM_TYPE_TOPIC, ROS_REPLY_TYPE_INVALID]; - message.sequence_id := 0; - - ! default values - message.mode := ROS_ROBOT_MODE_UNKNOWN; - message.e_stopped := ROS_TRISTATE_UNKNOWN; - message.drives_powered := ROS_TRISTATE_UNKNOWN; - message.error_code := ROS_TRISTATE_UNKNOWN; - message.in_error := ROS_TRISTATE_UNKNOWN; - message.in_motion := ROS_TRISTATE_UNKNOWN; - message.motion_possible := ROS_TRISTATE_UNKNOWN; - - ! Get operating mode - TEST OpMode() - CASE OP_AUTO: - message.mode := ROS_ROBOT_MODE_AUTO; - CASE OP_MAN_PROG, OP_MAN_TEST: - message.mode := ROS_ROBOT_MODE_MANUAL; - CASE OP_UNDEF: - message.mode := ROS_ROBOT_MODE_UNKNOWN; - ENDTEST - - ! Get E-stop status - IF DOutput(signalRobotEStop) = 1 THEN - message.e_stopped := ROS_TRISTATE_ON; - ELSE - message.e_stopped := ROS_TRISTATE_OFF; - ENDIF - - ! Get whether motors have power - IF DOutput(signalMotorOn) = 1 THEN - message.drives_powered := ROS_TRISTATE_TRUE; - ELSE - message.drives_powered := ROS_TRISTATE_FALSE; - ENDIF - - ! Determine in_error and set error_code if in_error is true - if DOutput(signalExecutionError) = 1 THEN - message.in_error := ROS_TRISTATE_TRUE; - message.error_code := ERRNO; - ELSE - message.in_error := ROS_TRISTATE_FALSE; - message.error_code := 0; - ENDIF - - ! Get in_motion - IF DOutput(signalRobotNotMoving) = 1 THEN - message.in_motion := ROS_TRISTATE_FALSE; - ELSE - message.in_motion := ROS_TRISTATE_TRUE; - ENDIF - - ! Get whether motion is possible - if (DOutput(signalMotionPossible) = 1) AND - (DOutput(signalRobotActive) = 1) AND - (DOutput(signalMotorOn) = 1) AND - (DOutput(signalRosMotionTaskExecuting) = 1) THEN - message.motion_possible := ROS_TRISTATE_TRUE; - ELSE - message.motion_possible := ROS_TRISTATE_FALSE; - ENDIF - - ! send message to client - ROS_send_msg_robot_status client_socket, message; - -ERROR - RAISE; ! raise errors to calling code -ENDPROC - -ENDMODULE diff --git a/abb_driver/src/abb_joint_downloader_node.cpp b/abb_driver/src/abb_joint_downloader_node.cpp deleted file mode 100644 index 5981b606..00000000 --- a/abb_driver/src/abb_joint_downloader_node.cpp +++ /dev/null @@ -1,86 +0,0 @@ -/* - * 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_driver/abb_utils.h" -#include "industrial_robot_client/joint_trajectory_downloader.h" -#include "industrial_utils/param_utils.h" - -using industrial_robot_client::joint_trajectory_downloader::JointTrajectoryDownloader; -namespace StandardSocketPorts = industrial::simple_socket::StandardSocketPorts; - -class ABB_JointTrajectoryDownloader : public JointTrajectoryDownloader -{ - using JointTrajectoryDownloader::init; // so base-class init() stays visible - - bool J23_coupled_; - -public: - bool init(std::string default_ip = "", int default_port = StandardSocketPorts::MOTION) - { - if (!JointTrajectoryDownloader::init(default_ip, default_port)) // call base-class init() - return false; - - if (ros::param::has("J23_coupled")) - ros::param::get("J23_coupled", this->J23_coupled_); - else - J23_coupled_ = false; - - return true; - } - - bool transform(const trajectory_msgs::JointTrajectoryPoint& pt_in, trajectory_msgs::JointTrajectoryPoint* pt_out) - { - // correct for parallel linkage effects, if desired - // - use POSITIVE factor for joint->motor correction - abb::utils::linkage_transform(pt_in, pt_out, J23_coupled_ ? +1:0 ); - - return true; - } - - bool calc_velocity(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_velocity) - { - *rbt_velocity = 0; // unused by ABB driver - return true; - } -}; - -int main(int argc, char** argv) -{ - // initialize node - ros::init(argc, argv, "motion_interface"); - - // launch the default JointTrajectoryDownloader connection/handlers - ABB_JointTrajectoryDownloader motionInterface; - motionInterface.init(); - motionInterface.run(); - - return 0; -} diff --git a/abb_driver/src/abb_robot_state_node.cpp b/abb_driver/src/abb_robot_state_node.cpp deleted file mode 100644 index 7c92d972..00000000 --- a/abb_driver/src/abb_robot_state_node.cpp +++ /dev/null @@ -1,81 +0,0 @@ -/* - * 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_driver/abb_utils.h" -#include "industrial_robot_client/robot_state_interface.h" -#include "industrial_utils/param_utils.h" - -using industrial_robot_client::robot_state_interface::RobotStateInterface; -using industrial_robot_client::joint_relay_handler::JointRelayHandler; - -class ABB_JointRelayHandler : public JointRelayHandler -{ - bool J23_coupled_; - -public: - ABB_JointRelayHandler() : JointRelayHandler() - { - if (ros::param::has("J23_coupled")) - ros::param::get("J23_coupled", this->J23_coupled_); - else - J23_coupled_ = false; - } - - bool transform(const std::vector& pos_in, std::vector* pos_out) - { - // correct for parallel linkage effects, if desired - // - use NEGATIVE factor for motor->joint correction - abb::utils::linkage_transform(pos_in, pos_out, J23_coupled_ ? -1:0 ); - - return true; - } -}; - -int main(int argc, char** argv) -{ - // initialize node - ros::init(argc, argv, "state_interface"); - - // launch the default Robot State Interface connection/handlers - RobotStateInterface rsi; - rsi.init(); - - // replace the JointRelayHandler with ABB-version - ABB_JointRelayHandler jointHandler; // for joint-linkage correction - std::vector joint_names = rsi.get_joint_names(); - jointHandler.init(rsi.get_connection(), joint_names); - rsi.add_handler(&jointHandler); - - // run the node - rsi.run(); - - return 0; -} diff --git a/abb_driver/src/abb_utils.cpp b/abb_driver/src/abb_utils.cpp deleted file mode 100644 index 29f2e2ae..00000000 --- a/abb_driver/src/abb_utils.cpp +++ /dev/null @@ -1,57 +0,0 @@ -/* - * 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_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& points_in, std::vector* 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