├── doc └── resources │ ├── fzi_logo.png │ └── ur_logo.jpg ├── test ├── launch │ ├── robot.launch │ ├── setup.launch │ └── hw_interface.launch ├── hw_interface │ ├── config.yaml │ ├── controllers.yaml │ ├── control_node.cpp │ ├── hw_interface.h │ └── hw_interface.cpp ├── cfg │ └── SpeedScaling.cfg ├── pass_through_controllers.test ├── test_forward_cartesian_trajectory.py └── test_forward_joint_trajectory.py ├── CHANGELOG.rst ├── .clang-tidy ├── pass_through_controllers_plugin.xml ├── .github └── workflows │ └── ci.yml ├── src └── pass_through_controllers.cpp ├── .clang-format ├── package.xml ├── CMakeLists.txt ├── include └── pass_through_controllers │ ├── pass_through_controllers.h │ ├── trajectory_interface.h │ └── pass_through_controllers.hpp ├── README.md └── LICENSE /doc/resources/fzi_logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UniversalRobots/Universal_Robots_ROS_passthrough_controllers/HEAD/doc/resources/fzi_logo.png -------------------------------------------------------------------------------- /doc/resources/ur_logo.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UniversalRobots/Universal_Robots_ROS_passthrough_controllers/HEAD/doc/resources/ur_logo.jpg -------------------------------------------------------------------------------- /test/launch/robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /test/hw_interface/config.yaml: -------------------------------------------------------------------------------- 1 | # HW interface configuration 2 | 3 | control_rate: 125 4 | 5 | joints: 6 | - joint1 7 | - joint2 8 | - joint3 9 | - joint4 10 | - joint5 11 | - joint6 12 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package pass_through_controllers 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.1.0 (2021-06-15) 6 | ------------------ 7 | * Initial release 8 | * Contributors: Felix Exner, Rune Søe-Knudsen, Stefan Scherzinger 9 | -------------------------------------------------------------------------------- /test/cfg/SpeedScaling.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "pass_through_controllers" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("speed_scaling", double_t, 0, "Speed scaling in the oem controller", 1.0, 0.0, 1.0) 9 | 10 | exit(gen.generate(PACKAGE, "pass_through_controllers", "SpeedScaling")) 11 | -------------------------------------------------------------------------------- /test/launch/setup.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /test/pass_through_controllers.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /.clang-tidy: -------------------------------------------------------------------------------- 1 | Checks: '-*,readability-identifier-naming' 2 | CheckOptions: 3 | - { key: readability-identifier-naming.NamespaceCase, value: lower_case } 4 | - { key: readability-identifier-naming.ClassCase, value: CamelCase } 5 | - { key: readability-identifier-naming.PrivateMemberSuffix, value: _ } 6 | - { key: readability-identifier-naming.StructCase, value: CamelCase } 7 | - { key: readability-identifier-naming.FunctionCase, value: camelBack } 8 | - { key: readability-identifier-naming.VariableCase, value: lower_case } 9 | - { key: readability-identifier-naming.GlobalVariablePrefix, value: g_ } 10 | - { key: readability-identifier-naming.GlobalConstantCase, value: UPPER_CASE } 11 | -------------------------------------------------------------------------------- /pass_through_controllers_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | This controller forwards joint trajectory goals to the robot for interpolation 8 | 9 | 10 | 11 | 14 | 15 | This controller forwards Cartesian trajectory goals to the robot for interpolation 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /test/hw_interface/controllers.yaml: -------------------------------------------------------------------------------- 1 | # A controller to forward joint trajectories to a native robot controller for 2 | # interpolation. 3 | forward_joint_trajectories: 4 | type: "pass_through_controllers/JointTrajectoryController" 5 | joints: 6 | - joint1 7 | - joint2 8 | - joint3 9 | - joint4 10 | - joint5 11 | - joint6 12 | 13 | # A controller to forward Cartesian trajectories to a native robot controller 14 | # for interpolation. 15 | forward_cartesian_trajectories: 16 | type: "pass_through_controllers/CartesianTrajectoryController" 17 | joints: 18 | - joint1 19 | - joint2 20 | - joint3 21 | - joint4 22 | - joint5 23 | - joint6 24 | 25 | # For testing resource conflicts with joint trajectory forwarding 26 | conflicting_joint_controller: 27 | type: "position_controllers/JointTrajectoryController" 28 | joints: 29 | - joint1 30 | - joint2 31 | - joint3 32 | - joint4 33 | - joint5 34 | - joint6 35 | 36 | # For testing resource conflicts with Cartesian trajectory forwarding 37 | conflicting_cartesian_controller: 38 | type: "position_controllers/JointTrajectoryController" 39 | joints: 40 | - joint1 41 | - joint2 42 | - joint3 43 | - joint4 44 | - joint5 45 | - joint6 46 | -------------------------------------------------------------------------------- /.github/workflows/ci.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | format_check: 7 | runs-on: ubuntu-latest 8 | 9 | steps: 10 | - uses: actions/checkout@v2 11 | - uses: 'ros-industrial/industrial_ci@master' 12 | env: 13 | ROS_DISTRO: noetic 14 | CLANG_FORMAT_CHECK: file 15 | CLANG_FORMAT_VERSION: "10" 16 | industrial_ci: 17 | strategy: 18 | fail-fast: false 19 | matrix: 20 | ros_distro: [ melodic, noetic ] 21 | ros_repo: [ main, testing ] 22 | env: 23 | CCACHE_DIR: "${{ github.workspace }}/.ccache" 24 | runs-on: ubuntu-latest 25 | steps: 26 | - uses: actions/checkout@v1 27 | - name: ccache cache 28 | uses: actions/cache@v2 29 | with: 30 | path: ${{ env.CCACHE_DIR }} 31 | # we always want the ccache cache to be persisted, as we cannot easily 32 | # determine whether dependencies have changed, and ccache will manage 33 | # updating the cache for us. Adding 'run_id' to the key will force an 34 | # upload at the end of the job. 35 | key: ccache-${{ matrix.ros_distro }}-${{ matrix.ros_repo }}-${{github.run_id}} 36 | restore-keys: | 37 | ccache-${{ matrix.ros_distro }}-${{ matrix.ros_repo }} 38 | - uses: 'ros-industrial/industrial_ci@master' 39 | env: 40 | ROS_DISTRO: ${{ matrix.ros_distro }} 41 | ROS_REPO: ${{ matrix.ros_repo }} 42 | 43 | -------------------------------------------------------------------------------- /src/pass_through_controllers.cpp: -------------------------------------------------------------------------------- 1 | // -- BEGIN LICENSE BLOCK ---------------------------------------------- 2 | // Copyright 2020 FZI Forschungszentrum Informatik 3 | // Created on behalf of Universal Robots A/S 4 | // 5 | // Licensed under the Apache License, Version 2.0 (the "License"); 6 | // you may not use this file except in compliance with the License. 7 | // You may obtain a copy of the License at 8 | // 9 | // http://www.apache.org/licenses/LICENSE-2.0 10 | // 11 | // Unless required by applicable law or agreed to in writing, software 12 | // distributed under the License is distributed on an "AS IS" BASIS, 13 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | // See the License for the specific language governing permissions and 15 | // limitations under the License. 16 | // -- END LICENSE BLOCK ------------------------------------------------ 17 | 18 | //----------------------------------------------------------------------------- 19 | /*!\file pass_through_controllers.cpp 20 | * 21 | * \author Stefan Scherzinger 22 | * \date 2020/10/13 23 | * 24 | */ 25 | //----------------------------------------------------------------------------- 26 | 27 | // Pluginlib 28 | #include 29 | 30 | // Project 31 | #include 32 | 33 | // Exports 34 | 35 | namespace pass_through_controllers 36 | { 37 | using JointTrajectoryController = 38 | trajectory_controllers::PassThroughController; 39 | 40 | using CartesianTrajectoryController = 41 | trajectory_controllers::PassThroughController; 42 | } // namespace pass_through_controllers 43 | 44 | PLUGINLIB_EXPORT_CLASS(pass_through_controllers::JointTrajectoryController, controller_interface::ControllerBase) 45 | 46 | PLUGINLIB_EXPORT_CLASS(pass_through_controllers::CartesianTrajectoryController, controller_interface::ControllerBase) 47 | -------------------------------------------------------------------------------- /test/launch/hw_interface.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 20 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | AccessModifierOffset: -2 4 | ConstructorInitializerIndentWidth: 2 5 | AlignEscapedNewlinesLeft: false 6 | AlignTrailingComments: true 7 | AllowAllParametersOfDeclarationOnNextLine: false 8 | AllowShortIfStatementsOnASingleLine: false 9 | AllowShortLoopsOnASingleLine: false 10 | AllowShortFunctionsOnASingleLine: None 11 | AllowShortLoopsOnASingleLine: false 12 | AlwaysBreakTemplateDeclarations: true 13 | AlwaysBreakBeforeMultilineStrings: false 14 | BreakBeforeBinaryOperators: false 15 | BreakBeforeTernaryOperators: false 16 | BreakConstructorInitializersBeforeComma: true 17 | BinPackParameters: true 18 | ColumnLimit: 120 19 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 20 | DerivePointerBinding: false 21 | PointerBindsToType: true 22 | ExperimentalAutoDetectBinPacking: false 23 | IndentCaseLabels: true 24 | MaxEmptyLinesToKeep: 1 25 | NamespaceIndentation: None 26 | ObjCSpaceBeforeProtocolList: true 27 | PenaltyBreakBeforeFirstCallParameter: 19 28 | PenaltyBreakComment: 60 29 | PenaltyBreakString: 1 30 | PenaltyBreakFirstLessLess: 1000 31 | PenaltyExcessCharacter: 1000 32 | PenaltyReturnTypeOnItsOwnLine: 90 33 | SpacesBeforeTrailingComments: 2 34 | Cpp11BracedListStyle: false 35 | Standard: Auto 36 | IndentWidth: 2 37 | TabWidth: 2 38 | UseTab: Never 39 | IndentFunctionDeclarationAfterType: false 40 | SpacesInParentheses: false 41 | SpacesInAngles: false 42 | SpaceInEmptyParentheses: false 43 | SpacesInCStyleCastParentheses: false 44 | SpaceAfterControlStatementKeyword: true 45 | SpaceBeforeAssignmentOperators: true 46 | ContinuationIndentWidth: 4 47 | SortIncludes: false 48 | SpaceAfterCStyleCast: false 49 | 50 | # Configure each individual brace in BraceWrapping 51 | BreakBeforeBraces: Custom 52 | 53 | # Control of individual brace wrapping cases 54 | BraceWrapping: { 55 | AfterClass: 'true' 56 | AfterControlStatement: 'true' 57 | AfterEnum : 'true' 58 | AfterFunction : 'true' 59 | AfterNamespace : 'true' 60 | AfterStruct : 'true' 61 | AfterUnion : 'true' 62 | BeforeCatch : 'true' 63 | BeforeElse : 'true' 64 | IndentBraces : 'false' 65 | } 66 | ... 67 | -------------------------------------------------------------------------------- /test/hw_interface/control_node.cpp: -------------------------------------------------------------------------------- 1 | // -- BEGIN LICENSE BLOCK ---------------------------------------------- 2 | // Copyright 2020 FZI Forschungszentrum Informatik 3 | // Created on behalf of Universal Robots A/S 4 | // 5 | // Licensed under the Apache License, Version 2.0 (the "License"); 6 | // you may not use this file except in compliance with the License. 7 | // You may obtain a copy of the License at 8 | // 9 | // http://www.apache.org/licenses/LICENSE-2.0 10 | // 11 | // Unless required by applicable law or agreed to in writing, software 12 | // distributed under the License is distributed on an "AS IS" BASIS, 13 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | // See the License for the specific language governing permissions and 15 | // limitations under the License. 16 | // -- END LICENSE BLOCK ------------------------------------------------ 17 | 18 | //----------------------------------------------------------------------------- 19 | /*!\file control_node.cpp 20 | * 21 | * \author Stefan Scherzinger 22 | * \date 2020/09/09 23 | * 24 | */ 25 | //----------------------------------------------------------------------------- 26 | 27 | // Ros 28 | #include 29 | 30 | // Ros control 31 | #include 32 | 33 | // This project 34 | #include "hw_interface.h" 35 | #include "ros/time.h" 36 | 37 | int main(int argc, char** argv) 38 | { 39 | ros::init(argc, argv, "hw_interface_example"); 40 | ros::NodeHandle nh; 41 | examples::HWInterface hw_interface; 42 | controller_manager::ControllerManager controller_manager(&hw_interface); 43 | 44 | int control_rate; 45 | if (!nh.getParam("control_rate", control_rate)) 46 | { 47 | control_rate = 125; 48 | ROS_INFO_STREAM("Failed to load 'control_rate' from parameter server. Using default " << control_rate); 49 | } 50 | 51 | // Control cycle for the robot 52 | ros::Rate rate(control_rate); 53 | 54 | ros::AsyncSpinner spinner(2); 55 | spinner.start(); 56 | 57 | while (ros::ok()) 58 | { 59 | auto period = rate.expectedCycleTime(); // use nominal cycle 60 | 61 | hw_interface.read(ros::Time::now(), period); 62 | hw_interface.write(ros::Time::now(), period); 63 | controller_manager.update(ros::Time::now(), period); 64 | 65 | rate.sleep(); 66 | } 67 | 68 | return 0; 69 | } 70 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | pass_through_controllers 5 | 0.1.0 6 | 7 | Trajectory controllers (joint-based and Cartesian) that forward trajectories 8 | directly to a robot controller and let it handle trajectory interpolation and execution. 9 | 10 | 11 | Stefan Scherzinger 12 | Felix Exner 13 | 14 | Apache-2.0 15 | 16 | http://wiki.ros.org/pass_through_controllers 17 | 18 | catkin 19 | 20 | actionlib 21 | cartesian_control_msgs 22 | cartesian_interface 23 | cartesian_trajectory_controller 24 | controller_interface 25 | controller_manager 26 | dynamic_reconfigure 27 | geometry_msgs 28 | hardware_interface 29 | pluginlib 30 | roscpp 31 | speed_scaling_interface 32 | trajectory_msgs 33 | 34 | control_msgs 35 | control_msgs 36 | 37 | actionlib 38 | actionlib_msgs 39 | cartesian_trajectory_controller 40 | control_msgs 41 | controller_manager_msgs 42 | joint_state_controller 43 | joint_trajectory_controller 44 | robot_state_publisher 45 | ros_control_boilerplate 46 | rospy 47 | rostest 48 | tf 49 | xacro 50 | 51 | 52 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /test/test_forward_cartesian_trajectory.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import unittest 3 | import copy 4 | import rospy 5 | import actionlib 6 | import sys 7 | from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal 8 | from cartesian_control_msgs.msg import ( 9 | FollowCartesianTrajectoryAction, 10 | FollowCartesianTrajectoryGoal, 11 | FollowCartesianTrajectoryResult, 12 | CartesianTrajectoryPoint) 13 | from trajectory_msgs.msg import JointTrajectoryPoint 14 | from controller_manager_msgs.srv import SwitchControllerRequest, SwitchController 15 | 16 | PKG = 'pass_through_controllers' 17 | NAME = 'test_cartesian_trajectory_forwarding' 18 | 19 | 20 | class IntegrationTest(unittest.TestCase): 21 | 22 | def __init__(self, *args): 23 | super(IntegrationTest, self).__init__(*args) 24 | 25 | rospy.init_node(NAME) 26 | 27 | timeout = rospy.Duration(3) 28 | 29 | self.set_joints = actionlib.SimpleActionClient( 30 | "/robot/joint_trajectory_controller/follow_joint_trajectory", FollowJointTrajectoryAction) 31 | if not self.set_joints.wait_for_server(timeout): 32 | self.fail("Could not reach robot action.") 33 | 34 | self.client = actionlib.SimpleActionClient( 35 | '/hw_interface/forward_cartesian_trajectories/follow_cartesian_trajectory', 36 | FollowCartesianTrajectoryAction) 37 | if not self.client.wait_for_server(timeout): 38 | self.fail("Could not reach hw_interface action.") 39 | 40 | self.switch_robot_ctrl = rospy.ServiceProxy('/robot/controller_manager/switch_controller', SwitchController) 41 | try: 42 | self.switch_robot_ctrl.wait_for_service(timeout.to_sec()) 43 | except rospy.exceptions.ROSException as err: 44 | self.fail( 45 | "Could not reach robot controller switch service. Msg: {}".format(err)) 46 | 47 | self.switch_forward_ctrl = rospy.ServiceProxy('/hw_interface/controller_manager/switch_controller', SwitchController) 48 | try: 49 | self.switch_forward_ctrl.wait_for_service(timeout.to_sec()) 50 | except rospy.exceptions.ROSException as err: 51 | self.fail( 52 | "Could not reach hw_interface controller switch service. Msg: {}".format(err)) 53 | 54 | def test_normal_execution(self): 55 | """ Test the basic functionality by moving on a straight line """ 56 | self.move_to_start() 57 | self.switch_to_cartesian_control() 58 | self.move() 59 | self.assertEqual(self.client.get_result().error_code, 60 | FollowCartesianTrajectoryResult.SUCCESSFUL) 61 | 62 | def move_to_start(self): 63 | srv = SwitchControllerRequest() 64 | srv.stop_controllers = ['cartesian_trajectory_controller'] 65 | srv.start_controllers = ['joint_trajectory_controller'] 66 | srv.strictness = SwitchControllerRequest.BEST_EFFORT 67 | self.switch_robot_ctrl(srv) 68 | 69 | q_start = [0, -2.0, 2.26, -0.2513274122872, 1.57, 0.0] # From base to tip 70 | start_joint_state = FollowJointTrajectoryGoal() 71 | start_joint_state.trajectory.joint_names = [ 72 | 'joint1', 73 | 'joint2', 74 | 'joint3', 75 | 'joint4', 76 | 'joint5', 77 | 'joint6'] 78 | 79 | start_joint_state.trajectory.points = [ 80 | JointTrajectoryPoint(positions=q_start, time_from_start=rospy.Duration(3.0))] 81 | self.set_joints.send_goal( 82 | start_joint_state) 83 | self.set_joints.wait_for_result() 84 | 85 | def switch_to_cartesian_control(self): 86 | """ Assume possibly running joint controllers""" 87 | 88 | # Prepare robot dummy 89 | srv = SwitchControllerRequest() 90 | srv.stop_controllers = ['joint_trajectory_controller'] 91 | srv.start_controllers = ['cartesian_trajectory_controller'] 92 | srv.strictness = SwitchControllerRequest.BEST_EFFORT 93 | self.switch_robot_ctrl(srv) 94 | 95 | # Prepare passthrough controller 96 | srv = SwitchControllerRequest() 97 | srv.stop_controllers = ['forward_joint_trajectories'] 98 | srv.start_controllers = ['forward_cartesian_trajectories'] 99 | srv.strictness = SwitchControllerRequest.BEST_EFFORT 100 | self.switch_forward_ctrl(srv) 101 | 102 | def move(self): 103 | """ Move on a straight line """ 104 | 105 | # Cartesian end-effector pose that corresponds to the start joint 106 | # configuration 107 | start = CartesianTrajectoryPoint() 108 | start.pose.position.x = 0.354 109 | start.pose.position.y = 0.180 110 | start.pose.position.z = 0.390 111 | start.pose.orientation.x = 0.502 112 | start.pose.orientation.y = 0.502 113 | start.pose.orientation.z = 0.498 114 | start.pose.orientation.w = 0.498 115 | 116 | duration = 5 117 | 118 | p1 = copy.deepcopy(start) 119 | p1.pose.position.z += 0.3 120 | p1.time_from_start = rospy.Duration(duration) 121 | 122 | goal = FollowCartesianTrajectoryGoal() 123 | goal.trajectory.points.append(p1) 124 | 125 | self.client.send_goal(goal) 126 | self.client.wait_for_result() 127 | 128 | 129 | if __name__ == '__main__': 130 | import rostest 131 | rostest.run(PKG, NAME, IntegrationTest, sys.argv) 132 | -------------------------------------------------------------------------------- /test/test_forward_joint_trajectory.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import unittest 3 | import rospy 4 | import actionlib 5 | import sys 6 | from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal, FollowJointTrajectoryResult 7 | from trajectory_msgs.msg import JointTrajectoryPoint 8 | from controller_manager_msgs.srv import SwitchControllerRequest, SwitchController 9 | import tf 10 | import time 11 | import copy 12 | from collections import OrderedDict 13 | 14 | PKG = 'pass_through_controllers' 15 | NAME = 'test_joint_trajectory_forwarding' 16 | 17 | 18 | class IntegrationTest(unittest.TestCase): 19 | 20 | def __init__(self, *args): 21 | super(IntegrationTest, self).__init__(*args) 22 | 23 | rospy.init_node(NAME) 24 | 25 | timeout = rospy.Duration(3) 26 | 27 | self.client = actionlib.SimpleActionClient( 28 | "/hw_interface/forward_joint_trajectories/follow_joint_trajectory", FollowJointTrajectoryAction) 29 | try: 30 | self.client.wait_for_server(timeout) 31 | except rospy.exceptions.ROSException as err: 32 | self.fail("Could not reach controller action. Msg: {}".format(err)) 33 | 34 | self.switch_robot_ctrl = rospy.ServiceProxy('/robot/controller_manager/switch_controller', SwitchController) 35 | try: 36 | self.switch_robot_ctrl.wait_for_service(timeout.to_sec()) 37 | except rospy.exceptions.ROSException as err: 38 | self.fail( 39 | "Could not reach robot controller switch service. Msg: {}".format(err)) 40 | 41 | self.switch_forward_ctrl = rospy.ServiceProxy('/hw_interface/controller_manager/switch_controller', SwitchController) 42 | try: 43 | self.switch_forward_ctrl.wait_for_service(timeout.to_sec()) 44 | except rospy.exceptions.ROSException as err: 45 | self.fail( 46 | "Could not reach hw_interface controller switch service. Msg: {}".format(err)) 47 | 48 | self.listener = tf.TransformListener() 49 | 50 | # Correctly ordered joint names with reference joint state 51 | self.joint_map = OrderedDict() 52 | self.joint_map['joint1'] = 0 53 | self.joint_map['joint2'] = -2.0 54 | self.joint_map['joint3'] = 2.26 55 | self.joint_map['joint4'] = -0.2513274122872 56 | self.joint_map['joint5'] = 1.57 57 | self.joint_map['joint6'] = 0.0 58 | 59 | # Cartesian reference pose for this joint state (x,y,z, qx, qy, qz, qw) 60 | self.p_ref = [0.354, 0.180, 0.390, 0.502, 0.502, 0.498, 0.498] 61 | 62 | def test_normal_execution(self): 63 | """ Test the basic functionality by moving to a defined joint state """ 64 | self.switch_to_joint_control() 65 | 66 | # Move to reference joint state 67 | start_joint_state = FollowJointTrajectoryGoal() 68 | start_joint_state.trajectory.joint_names = self.joint_map.keys() 69 | 70 | start_joint_state.trajectory.points = [ 71 | JointTrajectoryPoint(positions=self.joint_map.values(), time_from_start=rospy.Duration(3.0))] 72 | start_joint_state.goal_time_tolerance = rospy.Duration(1) 73 | self.client.send_goal(start_joint_state) 74 | self.client.wait_for_result() 75 | 76 | time.sleep(1) 77 | self.check_reached() 78 | self.assertEqual(self.client.get_result().error_code, 79 | FollowJointTrajectoryResult.SUCCESSFUL) 80 | 81 | def test_mixed_joint_order(self): 82 | """ Test whether a mixed-up joint order gets executed correctly """ 83 | self.switch_to_joint_control() 84 | 85 | # Move to reference joint state with different joint order 86 | joint_names = ['joint6', 'joint4', 'joint3', 'joint1', 'joint5', 'joint2'] 87 | q_start = [self.joint_map[i] for i in joint_names] 88 | 89 | start_joint_state = FollowJointTrajectoryGoal() 90 | start_joint_state.trajectory.joint_names = joint_names 91 | 92 | start_joint_state.trajectory.points = [ 93 | JointTrajectoryPoint(positions=q_start, time_from_start=rospy.Duration(3.0))] 94 | start_joint_state.goal_time_tolerance = rospy.Duration(1) 95 | self.client.send_goal(start_joint_state) 96 | self.client.wait_for_result() 97 | 98 | time.sleep(1) 99 | self.check_reached() 100 | self.assertEqual(self.client.get_result().error_code, 101 | FollowJointTrajectoryResult.SUCCESSFUL) 102 | 103 | def test_wrong_joint_names(self): 104 | """ Test whether a trajectory with wrong joint names fails """ 105 | self.switch_to_joint_control() 106 | 107 | # Unknown joint names 108 | joint_names = ['a', 'b', 'c', 'd', 'e', 'f'] 109 | 110 | start_joint_state = FollowJointTrajectoryGoal() 111 | start_joint_state.trajectory.joint_names = joint_names 112 | 113 | start_joint_state.trajectory.points = [ 114 | JointTrajectoryPoint(positions=self.joint_map.values(), time_from_start=rospy.Duration(3.0))] 115 | start_joint_state.goal_time_tolerance = rospy.Duration(1) 116 | self.client.send_goal(start_joint_state) 117 | self.client.wait_for_result() 118 | 119 | time.sleep(1) 120 | self.assertEqual(self.client.get_result().error_code, 121 | FollowJointTrajectoryResult.INVALID_GOAL) 122 | 123 | def switch_to_joint_control(self): 124 | """ Assume possibly running Cartesian controllers""" 125 | 126 | # Prepare robot dummy 127 | srv = SwitchControllerRequest() 128 | srv.stop_controllers = ['cartesian_trajectory_controller'] 129 | srv.start_controllers = ['joint_trajectory_controller'] 130 | srv.strictness = SwitchControllerRequest.BEST_EFFORT 131 | self.switch_robot_ctrl(srv) 132 | 133 | # Prepare passthrough controller 134 | srv = SwitchControllerRequest() 135 | srv.stop_controllers = ['forward_cartesian_trajectories'] 136 | srv.start_controllers = ['forward_joint_trajectories'] 137 | srv.strictness = SwitchControllerRequest.BEST_EFFORT 138 | self.switch_forward_ctrl(srv) 139 | 140 | def check_reached(self): 141 | """ Check whether we reached our target within TF lookup accuracy """ 142 | try: 143 | (trans, rot) = self.listener.lookupTransform('base_link', 'tool0', rospy.Time(0)) 144 | except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 145 | pass 146 | n = 3 # places accuracy 147 | self.assertAlmostEqual(self.p_ref[0], trans[0], n) 148 | self.assertAlmostEqual(self.p_ref[1], trans[1], n) 149 | self.assertAlmostEqual(self.p_ref[2], trans[2], n) 150 | self.assertAlmostEqual(self.p_ref[3], rot[0], n) 151 | self.assertAlmostEqual(self.p_ref[4], rot[1], n) 152 | self.assertAlmostEqual(self.p_ref[5], rot[2], n) 153 | self.assertAlmostEqual(self.p_ref[6], rot[3], n) 154 | 155 | 156 | if __name__ == '__main__': 157 | import rostest 158 | rostest.run(PKG, NAME, IntegrationTest, sys.argv) 159 | -------------------------------------------------------------------------------- /test/hw_interface/hw_interface.h: -------------------------------------------------------------------------------- 1 | // -- BEGIN LICENSE BLOCK ---------------------------------------------- 2 | // Copyright 2020 FZI Forschungszentrum Informatik 3 | // Created on behalf of Universal Robots A/S 4 | // 5 | // Licensed under the Apache License, Version 2.0 (the "License"); 6 | // you may not use this file except in compliance with the License. 7 | // You may obtain a copy of the License at 8 | // 9 | // http://www.apache.org/licenses/LICENSE-2.0 10 | // 11 | // Unless required by applicable law or agreed to in writing, software 12 | // distributed under the License is distributed on an "AS IS" BASIS, 13 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | // See the License for the specific language governing permissions and 15 | // limitations under the License. 16 | // -- END LICENSE BLOCK ------------------------------------------------ 17 | 18 | //----------------------------------------------------------------------------- 19 | /*!\file hw_interface.h 20 | * 21 | * \author Stefan Scherzinger 22 | * \date 2020/09/09 23 | * 24 | */ 25 | //----------------------------------------------------------------------------- 26 | 27 | #pragma once 28 | 29 | // ROS 30 | #include "cartesian_control_msgs/FollowCartesianTrajectoryResult.h" 31 | #include "control_msgs/FollowJointTrajectoryResult.h" 32 | #include "ros/publisher.h" 33 | #include "ros/subscriber.h" 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | // Joint-based control 44 | #include 45 | #include 46 | #include 47 | #include 48 | 49 | // Cartesian-based control 50 | #include 51 | #include 52 | #include 53 | #include 54 | 55 | // Dynamic reconfigure 56 | #include 57 | #include 58 | 59 | // Speed scaling 60 | #include 61 | 62 | // Other 63 | #include 64 | #include 65 | #include 66 | #include 67 | 68 | namespace examples 69 | { 70 | class HWInterface : public hardware_interface::RobotHW 71 | { 72 | public: 73 | HWInterface(); 74 | ~HWInterface(); 75 | 76 | void read(const ros::Time& time, const ros::Duration& period) override; 77 | void write(const ros::Time& time, const ros::Duration& period) override; 78 | 79 | private: 80 | /** 81 | * @brief Dummy implementation for joint interpolation on the robot 82 | * 83 | * Passes this trajectory straight to a 84 | * JointTrajectoryController to mimic external interpolation. 85 | * 86 | * @param trajectory The trajectory blob to forward to the vendor driver 87 | */ 88 | void startJointInterpolation(const hardware_interface::JointTrajectory& trajectory); 89 | 90 | /** 91 | * @brief Dummy implementation for Cartesian interpolation on the robot 92 | * 93 | * Passes this trajectory straight to a 94 | * CartesianTrajectoryController to mimic external interpolation. 95 | * 96 | * @param trajectory The trajectory blob to forward to the vendor driver 97 | */ 98 | void startCartesianInterpolation(const hardware_interface::CartesianTrajectory& trajectory); 99 | 100 | /** 101 | * @brief Dummy implementation for canceling a running joint interpolation on the robot 102 | * 103 | * Cancels the active goal of the JointTrajectoryController via preempt request. 104 | */ 105 | void cancelJointInterpolation(); 106 | 107 | /** 108 | * @brief Dummy implementation for canceling a running Cartesian interpolation on the robot 109 | * 110 | * Cancels the active goal of the CartesianTrajectoryController via preempt request. 111 | */ 112 | void cancelCartesianInterpolation(); 113 | 114 | //! Actuated joints in order from base to tip 115 | std::vector joint_names_; 116 | 117 | // Interfaces 118 | ros_controllers_cartesian::CartesianStateInterface cart_state_interface_; 119 | ros_controllers_cartesian::PoseCommandInterface pose_cmd_interface_; 120 | hardware_interface::JointStateInterface jnt_state_interface_; 121 | hardware_interface::PositionJointInterface jnt_pos_interface_; 122 | hardware_interface::JointTrajectoryInterface jnt_traj_interface_; 123 | hardware_interface::CartesianTrajectoryInterface cart_traj_interface_; 124 | scaled_controllers::SpeedScalingInterface speedsc_interface_; 125 | 126 | // Buffers 127 | std::vector cmd_; 128 | std::vector pos_; 129 | std::vector vel_; 130 | std::vector eff_; 131 | double speed_scaling_; 132 | geometry_msgs::Pose pose_cmd_; 133 | 134 | // Configuration 135 | std::string ref_frame_id_; 136 | std::string frame_id_; 137 | 138 | // Dynamic reconfigure 139 | using SpeedScalingConfig = pass_through_controllers::SpeedScalingConfig; 140 | 141 | /** 142 | * @brief Use dynamic reconfigure to mimic the driver's speed scaling 143 | * 144 | * Note: The speed scaling interface used is not made for thread safety. 145 | * In real driver code, that's not a problem, because robot drivers will 146 | * operate in synchronous read-update-write cycles. Here, we use this 147 | * interface under somewhat unrealistic "dynamic reconfigure" conditions for 148 | * testing purposes. 149 | * 150 | * @param config The speed scaling from 0 to 1.0 151 | * @param level Not used 152 | */ 153 | void dynamicReconfigureCallback(SpeedScalingConfig& config, uint32_t level); 154 | 155 | std::shared_ptr> reconfig_server_; 156 | dynamic_reconfigure::Server::CallbackType callback_type_; 157 | 158 | // States 159 | geometry_msgs::Pose cartesian_pose_; 160 | geometry_msgs::Twist cartesian_twist_; 161 | geometry_msgs::Accel cartesian_accel_; 162 | geometry_msgs::Accel cartesian_jerk_; 163 | 164 | // Handles 165 | std::vector joint_handles_; 166 | std::vector joint_state_handles_; 167 | 168 | // Robot connection and communication 169 | std::unique_ptr> joint_based_communication_; 170 | void handleJointFeedback(const control_msgs::FollowJointTrajectoryFeedbackConstPtr& feedback); 171 | 172 | void handleJointDone(const actionlib::SimpleClientGoalState& state, 173 | const control_msgs::FollowJointTrajectoryResultConstPtr& result); 174 | 175 | std::unique_ptr> 176 | cartesian_based_communication_; 177 | void handleCartesianFeedback(const cartesian_control_msgs::FollowCartesianTrajectoryFeedbackConstPtr& feedback); 178 | 179 | void handleCartesianDone(const actionlib::SimpleClientGoalState& state, 180 | const cartesian_control_msgs::FollowCartesianTrajectoryResultConstPtr& result); 181 | }; 182 | 183 | } // namespace examples 184 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(pass_through_controllers) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | actionlib 9 | cartesian_control_msgs 10 | cartesian_interface 11 | control_msgs 12 | controller_interface 13 | controller_manager 14 | dynamic_reconfigure 15 | geometry_msgs 16 | hardware_interface 17 | pluginlib 18 | roscpp 19 | speed_scaling_interface 20 | trajectory_msgs 21 | ) 22 | 23 | ## System dependencies are found with CMake's conventions 24 | # find_package(Boost REQUIRED COMPONENTS system) 25 | 26 | 27 | ## Uncomment this if the package has a setup.py. This macro ensures 28 | ## modules and global scripts declared therein get installed 29 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 30 | # catkin_python_setup() 31 | 32 | ################################################ 33 | ## Declare ROS messages, services and actions ## 34 | ################################################ 35 | 36 | ## To declare and build messages, services or actions from within this 37 | ## package, follow these steps: 38 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 39 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 40 | ## * In the file package.xml: 41 | ## * add a build_depend tag for "message_generation" 42 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 43 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 44 | ## but can be declared for certainty nonetheless: 45 | ## * add a exec_depend tag for "message_runtime" 46 | ## * In this file (CMakeLists.txt): 47 | ## * add "message_generation" and every package in MSG_DEP_SET to 48 | ## find_package(catkin REQUIRED COMPONENTS ...) 49 | ## * add "message_runtime" and every package in MSG_DEP_SET to 50 | ## catkin_package(CATKIN_DEPENDS ...) 51 | ## * uncomment the add_*_files sections below as needed 52 | ## and list every .msg/.srv/.action file to be processed 53 | ## * uncomment the generate_messages entry below 54 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 55 | 56 | ## Generate messages in the 'msg' folder 57 | # add_message_files( 58 | # FILES 59 | # Message1.msg 60 | # Message2.msg 61 | # ) 62 | 63 | ## Generate services in the 'srv' folder 64 | # add_service_files( 65 | # FILES 66 | # Service1.srv 67 | # Service2.srv 68 | # ) 69 | 70 | ## Generate actions in the 'action' folder 71 | # add_action_files( 72 | # FILES 73 | # Action1.action 74 | # Action2.action 75 | # ) 76 | 77 | ## Generate added messages and services with any dependencies listed here 78 | # generate_messages( 79 | # DEPENDENCIES 80 | # trajectory_msgs 81 | # ) 82 | 83 | ################################################ 84 | ## Declare ROS dynamic reconfigure parameters ## 85 | ################################################ 86 | 87 | ## To declare and build dynamic reconfigure parameters within this 88 | ## package, follow these steps: 89 | ## * In the file package.xml: 90 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 91 | ## * In this file (CMakeLists.txt): 92 | ## * add "dynamic_reconfigure" to 93 | ## find_package(catkin REQUIRED COMPONENTS ...) 94 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 95 | ## and list every .cfg file to be processed 96 | 97 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 98 | generate_dynamic_reconfigure_options( 99 | test/cfg/SpeedScaling.cfg 100 | ) 101 | 102 | ################################### 103 | ## catkin specific configuration ## 104 | ################################### 105 | ## The catkin_package macro generates cmake config files for your package 106 | ## Declare things to be passed to dependent projects 107 | ## INCLUDE_DIRS: uncomment this if your package contains header files 108 | ## LIBRARIES: libraries you create in this project that dependent projects also need 109 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 110 | ## DEPENDS: system dependencies of this project that dependent projects also need 111 | catkin_package( 112 | INCLUDE_DIRS 113 | include 114 | LIBRARIES 115 | ${PROJECT_NAME} 116 | CATKIN_DEPENDS 117 | actionlib 118 | cartesian_control_msgs 119 | cartesian_interface 120 | control_msgs 121 | controller_interface 122 | controller_manager 123 | dynamic_reconfigure 124 | geometry_msgs 125 | hardware_interface 126 | pluginlib 127 | roscpp 128 | speed_scaling_interface 129 | trajectory_msgs 130 | # DEPENDS system_lib 131 | ) 132 | 133 | ########### 134 | ## Build ## 135 | ########### 136 | 137 | ## Specify additional locations of header files 138 | ## Your package locations should be listed before other locations 139 | include_directories( 140 | include 141 | test/hw_interface 142 | ${catkin_INCLUDE_DIRS} 143 | ) 144 | 145 | ## Declare a C++ library 146 | add_library(${PROJECT_NAME} 147 | src/pass_through_controllers.cpp 148 | ) 149 | 150 | ## Add cmake target dependencies of the library 151 | ## as an example, code may need to be generated before libraries 152 | ## either from message generation or dynamic reconfigure 153 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 154 | 155 | ## Declare a C++ executable 156 | ## With catkin_make all packages are built within a single CMake context 157 | ## The recommended prefix ensures that target names across packages don't collide 158 | 159 | ## Rename C++ executable without prefix 160 | ## The above recommended prefix causes long target names, the following renames the 161 | ## target back to the shorter version for ease of user use 162 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 163 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 164 | 165 | ## Add cmake target dependencies of the executable 166 | ## same as for the library above 167 | 168 | ## Specify libraries to link a library or executable target against 169 | target_link_libraries(${PROJECT_NAME} 170 | ${catkin_LIBRARIES} 171 | ) 172 | 173 | ############# 174 | ## Install ## 175 | ############# 176 | 177 | # all install targets should use catkin DESTINATION variables 178 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 179 | 180 | ## Mark executable scripts (Python etc.) for installation 181 | ## in contrast to setup.py, you can choose the destination 182 | # catkin_install_python(PROGRAMS 183 | # scripts/my_python_script 184 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 185 | # ) 186 | 187 | ## Mark executables for installation 188 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 189 | # install(TARGETS ${PROJECT_NAME}_node 190 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 191 | # ) 192 | 193 | ## Mark libraries for installation 194 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 195 | install(TARGETS ${PROJECT_NAME} 196 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 197 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 198 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 199 | ) 200 | 201 | # Mark cpp header files for installation 202 | install(DIRECTORY include/${PROJECT_NAME}/ 203 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 204 | FILES_MATCHING PATTERN "*.h" 205 | PATTERN ".svn" EXCLUDE 206 | ) 207 | 208 | ## Mark other files for installation (e.g. launch and bag files, etc.) 209 | install(FILES 210 | ${PROJECT_NAME}_plugin.xml 211 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 212 | ) 213 | 214 | ############# 215 | ## Testing ## 216 | ############# 217 | 218 | if(CATKIN_ENABLE_TESTING) 219 | 220 | add_executable(hw_interface_example 221 | test/hw_interface/control_node.cpp 222 | test/hw_interface/hw_interface.cpp 223 | ) 224 | add_dependencies(hw_interface_example ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 225 | target_link_libraries(hw_interface_example ${catkin_LIBRARIES}) 226 | 227 | 228 | find_package(rostest REQUIRED) 229 | 230 | add_rostest(test/pass_through_controllers.test DEPENDENCIES hw_interface_example) 231 | endif() 232 | 233 | ## Add gtest based cpp test target and link libraries 234 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_pass_through_controllers.cpp) 235 | # if(TARGET ${PROJECT_NAME}-test) 236 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 237 | # endif() 238 | 239 | ## Add folders to be run by python nosetests 240 | # catkin_add_nosetests(test) 241 | -------------------------------------------------------------------------------- /include/pass_through_controllers/pass_through_controllers.h: -------------------------------------------------------------------------------- 1 | // -- BEGIN LICENSE BLOCK ---------------------------------------------- 2 | // Copyright 2020 FZI Forschungszentrum Informatik 3 | // Created on behalf of Universal Robots A/S 4 | // 5 | // Licensed under the Apache License, Version 2.0 (the "License"); 6 | // you may not use this file except in compliance with the License. 7 | // You may obtain a copy of the License at 8 | // 9 | // http://www.apache.org/licenses/LICENSE-2.0 10 | // 11 | // Unless required by applicable law or agreed to in writing, software 12 | // distributed under the License is distributed on an "AS IS" BASIS, 13 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | // See the License for the specific language governing permissions and 15 | // limitations under the License. 16 | // -- END LICENSE BLOCK ------------------------------------------------ 17 | 18 | //----------------------------------------------------------------------------- 19 | /*!\file pass_through_controllers.h 20 | * 21 | * \author Stefan Scherzinger 22 | * \date 2020/07/16 23 | * 24 | */ 25 | //----------------------------------------------------------------------------- 26 | 27 | #pragma once 28 | 29 | // ROS control 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | // Joint-based 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | // Cartesian 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | // Other 48 | #include 49 | #include 50 | #include 51 | #include 52 | 53 | namespace trajectory_controllers 54 | { 55 | struct JointBase 56 | { 57 | using Tolerance = std::vector; 58 | using TrajectoryPoint = trajectory_msgs::JointTrajectoryPoint; 59 | using TrajectoryFeedback = hardware_interface::JointTrajectoryFeedback; 60 | using FollowTrajectoryAction = control_msgs::FollowJointTrajectoryAction; 61 | using FollowTrajectoryResult = control_msgs::FollowJointTrajectoryResult; 62 | using GoalConstPtr = control_msgs::FollowJointTrajectoryGoalConstPtr; 63 | }; 64 | 65 | struct CartesianBase 66 | { 67 | using Tolerance = cartesian_control_msgs::CartesianTolerance; 68 | using TrajectoryPoint = cartesian_control_msgs::CartesianTrajectoryPoint; 69 | using TrajectoryFeedback = hardware_interface::CartesianTrajectoryFeedback; 70 | using FollowTrajectoryAction = cartesian_control_msgs::FollowCartesianTrajectoryAction; 71 | using FollowTrajectoryResult = cartesian_control_msgs::FollowCartesianTrajectoryResult; 72 | using GoalConstPtr = cartesian_control_msgs::FollowCartesianTrajectoryGoalConstPtr; 73 | }; 74 | 75 | /** 76 | * @brief A ROS controller for forwarding trajectories to a robot for interpolation 77 | * 78 | * Instead of interpolating between the waypoints itself, this driver passes 79 | * the complete trajectories down to the according HW interfaces, assuming that 80 | * the driver's implementation makes use of whichever components are suitable 81 | * for that specific robot. 82 | * 83 | * This controller implements a simple action server that provides the common 84 | * /follow_joint_trajectory or /follow_cartesian_trajectory action interface. 85 | * 86 | * Users specify this controller in their .yaml files with: 87 | * 88 | * \code{.yaml} 89 | * # Your joint-based passthrough controller 90 | * forward_joint_trajectories: 91 | * type: "pass_through_controllers/JointTrajectoryController" 92 | * ... 93 | * 94 | * 95 | * # Your Cartesian passthrough controller 96 | * forward_cartesian_trajectories: 97 | * type: "pass_through_controllers/CartesianTrajectoryController" 98 | * ... 99 | * \endcode 100 | * 101 | * @tparam TrajectoryInterface The type of trajectory interface used for this 102 | * controller. Either hardware_interface::JointTrajectoryInterface or 103 | * hardware_interface::CartesianTrajectoryInterface. 104 | */ 105 | template 106 | class PassThroughController 107 | : public controller_interface::MultiInterfaceController, 109 | public std::conditional::value, 110 | JointBase, CartesianBase>::type 111 | { 112 | public: 113 | PassThroughController() 114 | : controller_interface::MultiInterfaceController( 116 | true) // Make speed scaling optional 117 | { 118 | } 119 | 120 | // Alias for full qualifications of inherited types. 121 | // This enables a compact definition of member functions for both joint-based 122 | // and Cartesian-based PassThroughControllers. 123 | using Base = 124 | typename std::conditional::value, 125 | JointBase, CartesianBase>::type; 126 | 127 | bool init(hardware_interface::RobotHW* hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh); 128 | 129 | void starting(const ros::Time& time); 130 | 131 | void stopping(const ros::Time& time); 132 | 133 | void update(const ros::Time& time, const ros::Duration& period); 134 | 135 | /** 136 | * @brief Callback method for new action goals 137 | * 138 | * This method calls the \a setGoal() method from the TrajectoryInterface. 139 | * Implementers of the ROS-control HW can choose how the trajectory goal is 140 | * forwarded to the robot for interpolation. 141 | * 142 | * Things in detail: 143 | * - New goals are only accepted when the controller is running 144 | * - Switching the controller (stopping) cancels running goals 145 | * - The goal's success is monitored in update() 146 | * 147 | * Further info on how the simple action server works is given 148 | * here. 149 | * 150 | * @param goal The trajectory goal 151 | */ 152 | void executeCB(const typename Base::GoalConstPtr& goal); 153 | 154 | /** 155 | * @brief Callback method for preempt requests 156 | * 157 | * This method gets called on every preempt request that happens either 158 | * directly upon a client request or indirectly when receiving a new goal 159 | * while another is still active. 160 | * 161 | * This method calls the \a setCancel() method from the TrajectoryInterface. 162 | * The RobotHW should implement how this notification is handled by the robot 163 | * vendor control. 164 | * 165 | * Also check 166 | * this info. 167 | * on the simple action server's preemption policy: 168 | */ 169 | void preemptCB(); 170 | 171 | private: 172 | /** 173 | * @brief Container for easy time management 174 | * 175 | */ 176 | struct ActionDuration 177 | { 178 | ActionDuration() : target(0.0), current(0.0) 179 | { 180 | } 181 | 182 | ros::Duration target; ///< Target duration of the current action. 183 | ros::Duration current; ///< Real duration of the current action. 184 | }; 185 | 186 | /** 187 | * @brief Monitor the trajectory execution 188 | * 189 | * @param feedback The feedback to use for evaluating tolerances 190 | */ 191 | void monitorExecution(const typename Base::TrajectoryFeedback& feedback); 192 | 193 | /** 194 | * @brief Check if tolerances are met 195 | * 196 | * @param error The error to check 197 | * @param tolerances The tolerances to check against 198 | * 199 | * @return False if any of the errors exceeds its tolerance, else true 200 | */ 201 | bool withinTolerances(const typename Base::TrajectoryPoint& error, const typename Base::Tolerance& tolerances); 202 | 203 | /** 204 | * @brief Check if follow trajectory goals are valid 205 | * 206 | * @param goal The goal to check. 207 | * 208 | * @return True if goal is valid, false otherwise 209 | */ 210 | bool isValid(const typename Base::GoalConstPtr& goal); 211 | 212 | /** 213 | * @brief Will get called upon finishing the forwarded trajectory 214 | */ 215 | void doneCB(const hardware_interface::ExecutionState& state); 216 | 217 | std::atomic done_; 218 | ActionDuration action_duration_; 219 | std::unique_ptr speed_scaling_; 220 | std::vector joint_names_; 221 | typename Base::Tolerance path_tolerances_; 222 | typename Base::Tolerance goal_tolerances_; 223 | TrajectoryInterface* trajectory_interface_; ///* Resource managed by RobotHW 224 | std::unique_ptr > action_server_; 225 | }; 226 | 227 | } // namespace trajectory_controllers 228 | 229 | #include 230 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Pass-Through Controllers 2 | A package for ROS-controllers and HW Interface mechanisms to enable forwarding of trajectories (both 3 | joint-based and Cartesian) to the robot for interpolation. 4 | 5 | ## Rationale 6 | The idea behind these controllers is to enable forwarding (pass through) incoming trajectories directly 7 | to the robot and let the OEM driver-side take care of interpolating between the waypoints. 8 | This is useful if your application demands industrial scale trajectory execution, and you prefer not to interpolate on the ROS side. 9 | Having the complete trajectory available on the driver side can be beneficial regarding smoothness of execution and avoid 10 | issues related to sending (streaming) ad hoc commands, as is classically done with current ROS-control approaches. 11 | The controllers implement simple action servers for trajectory execution (both Cartesian and joint-based). 12 | They are meant to be light-weight and their functionality is deliberately 13 | limited to starting trajectories, canceling them, and providing real-time 14 | feedback on progress. 15 | 16 | Note that most of the work has to be done in the hardware abstraction of the robot. 17 | And, unfortunately, this is very robot specific and hard to generalize. It's 18 | somewhat similar to the _read()_ and _write()_ functions of ROS-control, which 19 | need to implement robot-specific protocols. 20 | This package provides the necessary HW interfaces to implement this feature. 21 | 22 | ## Controller .yaml 23 | An example config could look like this: 24 | ```yaml 25 | # A controller to forward joint trajectories to the robot 26 | forward_joint_trajectories: 27 | type: "pass_through_controllers/JointTrajectoryController" 28 | joints: 29 | - joint1 30 | - joint2 31 | - joint3 32 | - joint4 33 | - joint5 34 | - joint6 35 | 36 | # A controller to forward joint trajectories to the robot 37 | forward_cartesian_trajectories: 38 | type: "pass_through_controllers/CartesianTrajectoryController" 39 | joints: 40 | - joint1 41 | - joint2 42 | - joint3 43 | - joint4 44 | - joint5 45 | - joint6 46 | ``` 47 | 48 | ## Examples 49 | The [integration 50 | tests](https://github.com/UniversalRobots/Universal_Robots_ROS_passthrough_controllers/tree/main/test) from this package might give a first impression on how to use this controller. 51 | 52 | Secondly, the [`ur_robot_driver`](https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/tree/master/ur_robot_driver) 53 | uses those controllers. 54 | 55 | 56 | ## Adapting your RobotHW 57 | The pass_through_controllers expect either a ``JointTrajectoryInterface`` or a ``CartesianTrajectoryInterface``, depending on the trajectory to forward. 58 | Registering them in your RobotHW could look like this: 59 | ```c++ 60 | 61 | #include 62 | 63 | class YourRobot : public hardware_interface::RobotHW 64 | { 65 | ... 66 | 67 | // Callbacks for the pass_through_controllers' events 68 | void startJointInterpolation(const hardware_interface::JointTrajectory& trajectory); 69 | void startCartesianInterpolation(const hardware_interface::CartesianTrajectory& trajectory); 70 | void cancelJointInterpolation(); 71 | void cancelCartesianInterpolation(); 72 | 73 | // New HW interfaces for trajectory forwarding 74 | hardware_interface::JointTrajectoryInterface jnt_traj_interface_; 75 | hardware_interface::CartesianTrajectoryInterface cart_traj_interface_; 76 | } 77 | ``` 78 | And in the implementation: 79 | ```c++ 80 | 81 | YourRobot::YourRobot() 82 | { 83 | ... 84 | // Register callbacks for trajectory passthrough 85 | jnt_traj_interface_.registerGoalCallback( 86 | std::bind(&HardwareInterface::startJointInterpolation, this, std::placeholders::_1)); 87 | jnt_traj_interface_.registerCancelCallback(std::bind(&HardwareInterface::cancelInterpolation, this)); 88 | cart_traj_interface_.registerGoalCallback( 89 | std::bind(&HardwareInterface::startCartesianInterpolation, this, std::placeholders::_1)); 90 | cart_traj_interface_.registerCancelCallback(std::bind(&HardwareInterface::cancelInterpolation, this)); 91 | ... 92 | } 93 | 94 | void YourRobot::startJointInterpolation(const hardware_interface::JointTrajectory& trajectory) 95 | { 96 | // Robot-specific implementation here 97 | } 98 | 99 | void YourRobot::startCartesianInterpolation(const hardware_interface::CartesianTrajectory& trajectory) 100 | { 101 | // Robot-specific implementation here 102 | } 103 | 104 | void YourRobot::cancelJointInterpolation() 105 | { 106 | // Robot-specific implementation here 107 | } 108 | 109 | void YourRobot::cancelCartesianInterpolation() 110 | { 111 | // Robot-specific implementation here 112 | } 113 | 114 | // Once execution is finished on the robot, it should signal the driver about the finished 115 | // trajectory. This is vendor-specific code and the below is only an example. 116 | void trajectorySignalFromRobotReceived(robot_vendor::TrajectoryResult result) 117 | { 118 | hardware_interface::ExecutionState final_state; 119 | switch (result) 120 | { 121 | case robot_vendor::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS: 122 | { 123 | final_state = hardware_interface::ExecutionState::SUCCESS; 124 | break; 125 | } 126 | case urcl::control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED: 127 | { 128 | final_state = hardware_interface::ExecutionState::PREEMPTED; 129 | break; 130 | } 131 | case urcl::control::TrajectoryResult::TRAJECTORY_RESULT_FAILURE: 132 | { 133 | final_state = hardware_interface::ExecutionState::ABORTED; 134 | break; 135 | } 136 | default: 137 | { 138 | std::stringstream ss; 139 | ss << "Unknown trajectory result: " << urcl::toUnderlying(result); 140 | throw(std::invalid_argument(ss.str())); 141 | } 142 | } 143 | 144 | // If you track which controller is running... 145 | // You will have to know which interface to send the result 146 | if (joint_forward_controller_running_) 147 | { 148 | jnt_traj_interface_.setDone(final_state); 149 | } 150 | else if (cartesian_forward_controller_running_) 151 | { 152 | cart_traj_interface_.setDone(final_state); 153 | } 154 | else 155 | { 156 | ROS_ERROR_STREAM("Received forwarded trajectory result with no forwarding controller running."); 157 | } 158 | 159 | } 160 | 161 | 162 | ``` 163 | 164 | The pass_through_controllers check for these interfaces and call the registered 165 | callbacks upon receiving new action goals or preemption requests. The implementation is robot-specific. 166 | 167 | In order to provide feedback for the pass_through_controllers' action clients, 168 | you should periodically fill the feedback buffers: 169 | ```c++ 170 | if (joint_forward_controller_running_) 171 | { 172 | jnt_traj_interface_.setFeedback(feedback); 173 | } 174 | if (cartesian_forward_controller_running_) 175 | { 176 | cart_traj_interface_.setFeedback(feedback); 177 | } 178 | 179 | ``` 180 | 181 | ## FAQ 182 | The passthrough controllers forward the trajectories to the robot for interpolation. 183 | On the ROS side, the possibilities to interfere with a running action are fairly limited and you can only start or preempt the trajectory execution. 184 | On the robot side, however, there will be additional ways to interfere with the execution that mostly depend on the OEM robot program and the implementation of the respective driver. 185 | The passthrough controllers support the additional concept of speed scaling, which can be used to pause trajectories. 186 | 187 | The following examples explain the passthrough controllers' behavior with the Universal Robots [`ur_robot_driver`](http://wiki.ros.org/ur_robot_driver). 188 | 189 | ### What happens when you hit the emergency stop? 190 | The trajectory execution is paused in this case by internally setting the speed scaling to zero. Once the program resumes, the trajectory execution continues. 191 | The passthrough controllers will wait until the execution finishes but will report an execution failure due to missing the waypoints' time requirements if a goal tolerance is specified. 192 | 193 | ### What happens when you power-off the system? 194 | This aborts the current trajectory execution. 195 | 196 | ### What happens when the robot goes into protective stop? 197 | This is handled similar to hitting the emergency stop. The speed scaling is set 198 | to zero and the trajectory execution continues once the protective 199 | stop is resolved. 200 | 201 | ### What happens when you stop the program? 202 | This stops all ROS-controllers and aborts the current trajectory execution. 203 | 204 | ## Acknowledgement 205 | Developed in collaboration between: 206 | 207 | [Universal Robots A/S](https://www.universal-robots.com/)   and   208 | [FZI Research Center for Information Technology](https://www.fzi.de). 209 | 210 | *** 211 | 215 | 216 | 217 | rosin_logo 219 | 220 | 221 | Supported by ROSIN - ROS-Industrial Quality-Assured Robot Software Components. 222 | More information: rosin-project.eu 223 | 224 | eu_flag 226 | 227 | This project has received funding from the European Union’s Horizon 2020 228 | research and innovation programme under grant agreement no. 732287. 229 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /test/hw_interface/hw_interface.cpp: -------------------------------------------------------------------------------- 1 | // -- BEGIN LICENSE BLOCK ---------------------------------------------- 2 | // Copyright 2020 FZI Forschungszentrum Informatik 3 | // Created on behalf of Universal Robots A/S 4 | // 5 | // Licensed under the Apache License, Version 2.0 (the "License"); 6 | // you may not use this file except in compliance with the License. 7 | // You may obtain a copy of the License at 8 | // 9 | // http://www.apache.org/licenses/LICENSE-2.0 10 | // 11 | // Unless required by applicable law or agreed to in writing, software 12 | // distributed under the License is distributed on an "AS IS" BASIS, 13 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | // See the License for the specific language governing permissions and 15 | // limitations under the License. 16 | // -- END LICENSE BLOCK ------------------------------------------------ 17 | 18 | //----------------------------------------------------------------------------- 19 | /*!\file hw_interface.cpp 20 | * 21 | * \author Stefan Scherzinger 22 | * \date 2020/09/09 23 | * 24 | */ 25 | //----------------------------------------------------------------------------- 26 | 27 | #include "hw_interface.h" 28 | #include "actionlib/client/simple_action_client.h" 29 | #include "actionlib/client/simple_client_goal_state.h" 30 | #include "actionlib/client/simple_goal_state.h" 31 | #include "cartesian_interface/cartesian_command_interface.h" 32 | #include "control_msgs/FollowJointTrajectoryAction.h" 33 | #include "control_msgs/FollowJointTrajectoryActionGoal.h" 34 | #include "control_msgs/FollowJointTrajectoryFeedback.h" 35 | #include "control_msgs/FollowJointTrajectoryGoal.h" 36 | #include "control_msgs/FollowJointTrajectoryResult.h" 37 | #include "pass_through_controllers/trajectory_interface.h" 38 | #include "ros/duration.h" 39 | #include 40 | 41 | namespace examples 42 | { 43 | HWInterface::HWInterface() 44 | { 45 | // Get names of controllable joints from the parameter server 46 | ros::NodeHandle nh; 47 | if (!nh.getParam("joints", joint_names_)) 48 | { 49 | ROS_ERROR_STREAM("Failed to load " << nh.getNamespace() + "/joints" 50 | << " from parameter server"); 51 | throw std::logic_error("Failed to initialize ros control."); 52 | } 53 | 54 | // Current UR driver convention 55 | ref_frame_id_ = "base"; 56 | frame_id_ = "tool0_controller"; 57 | 58 | // Connect dynamic reconfigure and overwrite the default values with values 59 | // on the parameter server. This is done automatically if parameters with 60 | // the according names exist. 61 | callback_type_ = 62 | std::bind(&HWInterface::dynamicReconfigureCallback, this, std::placeholders::_1, std::placeholders::_2); 63 | 64 | reconfig_server_ = std::make_shared >(nh); 65 | reconfig_server_->setCallback(callback_type_); 66 | 67 | const int nr_joints = joint_names_.size(); 68 | cmd_.resize(nr_joints); 69 | pos_.resize(nr_joints); 70 | vel_.resize(nr_joints); 71 | eff_.resize(nr_joints); 72 | 73 | // Initialize and register joint state handles 74 | for (int i = 0; i < nr_joints; ++i) 75 | { 76 | joint_state_handles_.push_back(hardware_interface::JointStateHandle(joint_names_[i], &pos_[i], &vel_[i], &eff_[i])); 77 | 78 | jnt_state_interface_.registerHandle(joint_state_handles_[i]); 79 | } 80 | registerInterface(&jnt_state_interface_); 81 | 82 | // Initialize and register a Cartesian state handle 83 | ros_controllers_cartesian::CartesianStateHandle cartesian_state_handle = 84 | ros_controllers_cartesian::CartesianStateHandle(ref_frame_id_, frame_id_, &cartesian_pose_, &cartesian_twist_, 85 | &cartesian_accel_, &cartesian_jerk_); 86 | cart_state_interface_.registerHandle(cartesian_state_handle); 87 | registerInterface(&cart_state_interface_); 88 | 89 | // Initialize and register a Cartesian pose command handle 90 | ros_controllers_cartesian::PoseCommandHandle pose_cmd_handle = 91 | ros_controllers_cartesian::PoseCommandHandle(cartesian_state_handle, &pose_cmd_); 92 | pose_cmd_interface_.registerHandle(pose_cmd_handle); 93 | registerInterface(&pose_cmd_interface_); 94 | 95 | // Initialize and register joint position command handles. 96 | for (int i = 0; i < nr_joints; ++i) 97 | { 98 | joint_handles_.push_back( 99 | hardware_interface::JointHandle(jnt_state_interface_.getHandle(joint_names_[i]), &cmd_[i])); 100 | 101 | jnt_pos_interface_.registerHandle(joint_handles_[i]); 102 | } 103 | registerInterface(&jnt_pos_interface_); 104 | 105 | // Initialize and prepare joint trajectory interface for PassThroughControllers 106 | jnt_traj_interface_.registerGoalCallback( 107 | std::bind(&HWInterface::startJointInterpolation, this, std::placeholders::_1)); 108 | jnt_traj_interface_.registerCancelCallback(std::bind(&HWInterface::cancelJointInterpolation, this)); 109 | registerInterface(&jnt_traj_interface_); 110 | 111 | // Initialize and prepare Cartesian trajectory interface for PassThroughControllers 112 | cart_traj_interface_.registerGoalCallback( 113 | std::bind(&HWInterface::startCartesianInterpolation, this, std::placeholders::_1)); 114 | cart_traj_interface_.registerCancelCallback(std::bind(&HWInterface::cancelCartesianInterpolation, this)); 115 | registerInterface(&cart_traj_interface_); 116 | 117 | // Initialize and register speed scaling. 118 | // Note: The handle's name is a convention. 119 | // ROS-controllers will use this name when calling getHandle(). 120 | speedsc_interface_.registerHandle(scaled_controllers::SpeedScalingHandle("speed_scaling_factor", &speed_scaling_)); 121 | registerInterface(&speedsc_interface_); 122 | 123 | // Robot dummy communication 124 | joint_based_communication_ = 125 | std::make_unique >("joint_trajectory_" 126 | "controller/" 127 | "follow_joint_" 128 | "trajectory", 129 | true); 130 | 131 | cartesian_based_communication_ = 132 | std::make_unique >("cartes" 133 | "ian_" 134 | "trajec" 135 | "tory_" 136 | "contro" 137 | "ller/" 138 | "follow" 139 | "_carte" 140 | "sian_" 141 | "trajec" 142 | "tory", 143 | true); 144 | 145 | if (!joint_based_communication_->waitForServer(ros::Duration(10)) || 146 | !cartesian_based_communication_->waitForServer(ros::Duration(10))) 147 | { 148 | ROS_ERROR("Trajectory action interfaces of the robot dummy are not available."); 149 | return; 150 | }; 151 | 152 | ROS_INFO("Example HW interface is ready"); 153 | } 154 | 155 | HWInterface::~HWInterface() 156 | { 157 | } 158 | 159 | void HWInterface::read(const ros::Time& time, const ros::Duration& period) 160 | { 161 | // Code for conventional ROS-control loop here. 162 | } 163 | 164 | void HWInterface::write(const ros::Time& time, const ros::Duration& period) 165 | { 166 | // Code for conventional ROS-control loop here. 167 | } 168 | 169 | void HWInterface::startJointInterpolation(const hardware_interface::JointTrajectory& trajectory) 170 | { 171 | joint_based_communication_->sendGoal( 172 | trajectory, std::bind(&HWInterface::handleJointDone, this, std::placeholders::_1, std::placeholders::_2), 173 | 0, // no active callback 174 | std::bind(&HWInterface::handleJointFeedback, this, std::placeholders::_1)); // Process feedback continuously 175 | } 176 | 177 | void HWInterface::startCartesianInterpolation(const hardware_interface::CartesianTrajectory& trajectory) 178 | { 179 | cartesian_based_communication_->sendGoal( 180 | trajectory, std::bind(&HWInterface::handleCartesianDone, this, std::placeholders::_1, std::placeholders::_2), 181 | 0, // no active callback 182 | std::bind(&HWInterface::handleCartesianFeedback, this, std::placeholders::_1)); // Process feedback continuously 183 | } 184 | 185 | void HWInterface::cancelJointInterpolation() 186 | { 187 | joint_based_communication_->cancelGoal(); 188 | 189 | // For your driver implementation, you might want to wait here for the robot to 190 | // actually cancel the execution. 191 | 192 | jnt_traj_interface_.setDone(hardware_interface::ExecutionState::PREEMPTED); 193 | } 194 | 195 | void HWInterface::cancelCartesianInterpolation() 196 | { 197 | cartesian_based_communication_->cancelGoal(); 198 | 199 | // For your driver implementation, you might want to wait here for the robot to 200 | // actually cancel the execution. 201 | 202 | cart_traj_interface_.setDone(hardware_interface::ExecutionState::PREEMPTED); 203 | } 204 | 205 | void HWInterface::dynamicReconfigureCallback(SpeedScalingConfig& config, uint32_t level) 206 | { 207 | // Let's hope for "thread safety" with fundamental types. 208 | speed_scaling_ = config.speed_scaling; 209 | } 210 | 211 | void HWInterface::handleJointFeedback(const control_msgs::FollowJointTrajectoryFeedbackConstPtr& feedback) 212 | { 213 | jnt_traj_interface_.setFeedback(*feedback); 214 | } 215 | 216 | void HWInterface::handleJointDone(const actionlib::SimpleClientGoalState& state, 217 | const control_msgs::FollowJointTrajectoryResultConstPtr& result) 218 | { 219 | if (state == actionlib::SimpleClientGoalState::PREEMPTED) 220 | { 221 | jnt_traj_interface_.setDone(hardware_interface::ExecutionState::PREEMPTED); 222 | return; 223 | } 224 | 225 | if (result->error_code == control_msgs::FollowJointTrajectoryResult::SUCCESSFUL) 226 | { 227 | jnt_traj_interface_.setDone(hardware_interface::ExecutionState::SUCCESS); 228 | return; 229 | } 230 | 231 | jnt_traj_interface_.setDone(hardware_interface::ExecutionState::ABORTED); 232 | } 233 | 234 | void HWInterface::handleCartesianFeedback( 235 | const cartesian_control_msgs::FollowCartesianTrajectoryFeedbackConstPtr& feedback) 236 | { 237 | cart_traj_interface_.setFeedback(*feedback); 238 | } 239 | 240 | void HWInterface::handleCartesianDone(const actionlib::SimpleClientGoalState& state, 241 | const cartesian_control_msgs::FollowCartesianTrajectoryResultConstPtr& result) 242 | { 243 | if (state == actionlib::SimpleClientGoalState::PREEMPTED) 244 | { 245 | cart_traj_interface_.setDone(hardware_interface::ExecutionState::PREEMPTED); 246 | return; 247 | } 248 | 249 | if (result->error_code == control_msgs::FollowJointTrajectoryResult::SUCCESSFUL) 250 | { 251 | cart_traj_interface_.setDone(hardware_interface::ExecutionState::SUCCESS); 252 | return; 253 | } 254 | 255 | cart_traj_interface_.setDone(hardware_interface::ExecutionState::ABORTED); 256 | } 257 | 258 | } // namespace examples 259 | -------------------------------------------------------------------------------- /include/pass_through_controllers/trajectory_interface.h: -------------------------------------------------------------------------------- 1 | // -- BEGIN LICENSE BLOCK ---------------------------------------------- 2 | // Copyright 2020 FZI Forschungszentrum Informatik 3 | // Created on behalf of Universal Robots A/S 4 | // 5 | // Licensed under the Apache License, Version 2.0 (the "License"); 6 | // you may not use this file except in compliance with the License. 7 | // You may obtain a copy of the License at 8 | // 9 | // http://www.apache.org/licenses/LICENSE-2.0 10 | // 11 | // Unless required by applicable law or agreed to in writing, software 12 | // distributed under the License is distributed on an "AS IS" BASIS, 13 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | // See the License for the specific language governing permissions and 15 | // limitations under the License. 16 | // -- END LICENSE BLOCK ------------------------------------------------ 17 | 18 | //----------------------------------------------------------------------------- 19 | /*!\file trajectory_interface.h 20 | * 21 | * \author Stefan Scherzinger 22 | * \date 2020/07/15 23 | * 24 | */ 25 | //----------------------------------------------------------------------------- 26 | 27 | #pragma once 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | namespace hardware_interface 40 | { 41 | /** 42 | * @brief Hardware-generic done flags for trajectory execution 43 | * 44 | * When forwarding trajectories to robots, we hand-over control of the actual 45 | * execution. This is a minimal set of generic flags to be reported by the 46 | * hardware that PassThroughControllers can process and react upon. 47 | */ 48 | enum class ExecutionState 49 | { 50 | SUCCESS = 0, 51 | PREEMPTED = -1, 52 | ABORTED = -2, 53 | }; 54 | 55 | /** 56 | * @brief TrajectoryType for joint-based trajectories 57 | */ 58 | using JointTrajectory = control_msgs::FollowJointTrajectoryGoal; 59 | 60 | /** 61 | * @brief TrajectoryType for Cartesian trajectories 62 | */ 63 | using CartesianTrajectory = cartesian_control_msgs::FollowCartesianTrajectoryGoal; 64 | 65 | /** 66 | * @brief FeedbackType for joint-based trajectories 67 | */ 68 | using JointTrajectoryFeedback = control_msgs::FollowJointTrajectoryFeedback; 69 | 70 | /** 71 | * @brief FeedbackType for Cartesian trajectories 72 | */ 73 | using CartesianTrajectoryFeedback = cartesian_control_msgs::FollowCartesianTrajectoryFeedback; 74 | 75 | /** 76 | * @brief Hardware interface for forwarding trajectories 77 | * 78 | * This special hardware interface is primarily used by PassThroughControllers, 79 | * which forward full trajectories to robots for interpolation. In contrast to 80 | * other hardware interfaces, this interface does not provide the usual handle 81 | * mechanism. Instead, callbacks and respective setter methods allow to 82 | * implement control flow during trajectory execution. Resources need to be 83 | * claimed in the forwarding controller's constructor with \a setResources(). 84 | * 85 | * The intended usage is as follows: 86 | * - The RobotHW instantiates this TrajectoryInterface and registers callbacks 87 | * for forwarded goals and cancel requests with \a registerGoalCallback() and 88 | * \a registerCancelCallback(), respectively. 89 | * - On the controller side, the PassThroughController calls the respective \a 90 | * setGoal() and \a setCancel() methods to trigger those events for the 91 | * RobotHW. The PassThroughController also registers a callback for signals 92 | * from the RobotHW when trajectory execution is done with \a 93 | * registerDoneCallback(). 94 | * - When done with the trajectory execution, the RobotHW calls \a setDone() to 95 | * inform the PassThroughController via its registered callback. 96 | * - The RobotHW gives feedback continuously from the execution with \a 97 | * setFeedback(), which is used by the PassThroughController via \a 98 | * getFeedback(). 99 | * 100 | * @tparam TrajectoryType Distinguish between joint-based and Cartesian trajectories 101 | * @tparam FeedbackType The type of feedback for the trajectory used. 102 | */ 103 | template 104 | class TrajectoryInterface : public hardware_interface::HardwareInterface 105 | { 106 | public: 107 | /** 108 | * @brief Register a RobotHW-side callback for new trajectory execution 109 | * 110 | * Callback for triggering execution start in the RobotHW. 111 | * Use this callback mechanism to handle starting of 112 | * trajectories on the robot vendor controller. 113 | * 114 | * @param f The function to be called for starting trajectory execution 115 | */ 116 | void registerGoalCallback(std::function f) 117 | { 118 | goal_callback_ = f; 119 | } 120 | 121 | /** 122 | * @brief Register a RobotHW-side callback for canceling requests 123 | * 124 | * @param f The function to be called for canceling trajectory execution 125 | */ 126 | void registerCancelCallback(std::function f) 127 | { 128 | cancel_callback_ = f; 129 | } 130 | 131 | /** 132 | * @brief Register a Controller-side callback for done signals from the hardware 133 | * 134 | * Use this mechanism in the PassThroughController for handling the 135 | * ExecutionState of the forwarded trajectory. 136 | * 137 | * @param f The function to be called when trajectory execution is done 138 | */ 139 | void registerDoneCallback(std::function f) 140 | { 141 | done_callback_ = f; 142 | } 143 | 144 | /** 145 | * @brief Start the forwarding of new trajectories 146 | * 147 | * Controller-side method to send incoming trajectories to the RobotHW. 148 | * 149 | * Note: The JointTrajectory type reorders the joints according to the 150 | * given joint resource list. 151 | * 152 | * @param command The new trajectory 153 | * @return True if goal is feasible, false otherwise 154 | */ 155 | bool setGoal(TrajectoryType goal); 156 | 157 | /** 158 | * @brief Cancel the current trajectory execution 159 | * 160 | * Controller-side method to cancel current trajectory execution on the robot. 161 | */ 162 | void setCancel() 163 | { 164 | if (cancel_callback_ != nullptr) 165 | cancel_callback_(); 166 | }; 167 | 168 | /** 169 | * @brief RobotHW-side method to mark the execution of a trajectory done. 170 | * 171 | * Call this function when done with a forwarded trajectory in your 172 | * RobotHW or when unexpected interruptions occur. The 173 | * PassThroughController will implement a callback to set appropriate 174 | * result flags for the trajectory action clients. 175 | * 176 | * @param state The final state after trajectory execution 177 | */ 178 | void setDone(const ExecutionState& state) 179 | { 180 | if (done_callback_ != nullptr) 181 | done_callback_(state); 182 | } 183 | 184 | /** 185 | * @brief Set trajectory feedback for PassThroughControllers 186 | * 187 | * This should be used by the RobotHW to provide continuous feedback on 188 | * trajectory execution for the PassThroughControllers. 189 | * 190 | * @param feedback The feedback content to write to the interface 191 | */ 192 | void setFeedback(FeedbackType feedback) 193 | { 194 | feedback_ = feedback; 195 | } 196 | 197 | /** 198 | * @brief Get trajectory feedback 199 | * 200 | * This can be used by PassThroughControllers to continuously poll 201 | * trajectory feedback from the hardware interface. 202 | * 203 | * @return The most recent feedback on the current trajectory execution 204 | */ 205 | FeedbackType getFeedback() const 206 | { 207 | return feedback_; 208 | } 209 | 210 | /** 211 | * @brief Get the joint names (resources) associated with this interface. 212 | * 213 | * @return Joint names 214 | */ 215 | std::vector getJointNames() const 216 | { 217 | return joint_names_; 218 | } 219 | 220 | /** 221 | * @brief Associate resources with this interface 222 | * 223 | * \Note: Call this inside the PassThroughController's constructor to 224 | * manage resource conflicts with other ROS controllers. 225 | * 226 | * @param resources A list of resource names 227 | */ 228 | void setResources(std::vector resources) 229 | { 230 | for (const std::string& joint : resources) 231 | { 232 | hardware_interface::HardwareInterface::claim(joint); 233 | } 234 | joint_names_ = resources; 235 | } 236 | 237 | private: 238 | std::function goal_callback_; 239 | std::function cancel_callback_; 240 | std::function done_callback_; 241 | FeedbackType feedback_; 242 | std::vector joint_names_; 243 | }; 244 | 245 | // Full spezialization for JointTrajectory 246 | template <> 247 | inline bool TrajectoryInterface::setGoal(JointTrajectory goal) 248 | { 249 | control_msgs::FollowJointTrajectoryGoal tmp = goal; 250 | 251 | // Respect joint order by computing the map between msg indices to expected indices. 252 | // If msg is {A, C, D, B} and expected is {A, B, C, D}, the associated mapping vector is {0, 2, 3, 1} 253 | auto msg = goal.trajectory.joint_names; 254 | auto expected = joint_names_; 255 | std::vector msg_joints(msg.size()); 256 | if (msg.size() != expected.size()) 257 | { 258 | // msg must contain all joint names. 259 | ROS_WARN("Not forwarding trajectory. It contains wrong number of joints"); 260 | return false; 261 | } 262 | for (auto msg_it = msg.begin(); msg_it != msg.end(); ++msg_it) 263 | { 264 | auto expected_it = std::find(expected.begin(), expected.end(), *msg_it); 265 | if (expected.end() == expected_it) 266 | { 267 | ROS_WARN_STREAM("Not forwarding trajectory. It contains at least one unexpected joint name: " << *msg_it); 268 | return false; 269 | } 270 | else 271 | { 272 | const size_t msg_dist = std::distance(msg.begin(), msg_it); 273 | const size_t expected_dist = std::distance(expected.begin(), expected_it); 274 | msg_joints[msg_dist] = expected_dist; 275 | } 276 | } 277 | 278 | // Reorder the joint names and data fields in all trajectory points 279 | tmp.trajectory.joint_names = expected; 280 | tmp.trajectory.points.clear(); 281 | 282 | for (auto point : goal.trajectory.points) 283 | { 284 | trajectory_msgs::JointTrajectoryPoint p{ point }; // init for equal data size 285 | 286 | for (size_t i = 0; i < expected.size(); ++i) 287 | { 288 | auto jnt_id = msg_joints[i]; 289 | 290 | if (point.positions.size() == expected.size()) 291 | p.positions[jnt_id] = point.positions[i]; 292 | if (point.velocities.size() == expected.size()) 293 | p.velocities[jnt_id] = point.velocities[i]; 294 | if (point.accelerations.size() == expected.size()) 295 | p.accelerations[jnt_id] = point.accelerations[i]; 296 | if (point.effort.size() == expected.size()) 297 | p.effort[jnt_id] = point.effort[i]; 298 | } 299 | tmp.trajectory.points.push_back(p); 300 | } 301 | 302 | if (goal_callback_ != nullptr) 303 | { 304 | goal_callback_(tmp); 305 | return true; 306 | } 307 | return false; 308 | } 309 | 310 | // Full spezialization for CartesianTrajectory 311 | template <> 312 | inline bool TrajectoryInterface::setGoal(CartesianTrajectory goal) 313 | { 314 | if (goal_callback_ != nullptr) 315 | { 316 | goal_callback_(goal); 317 | return true; 318 | } 319 | return false; 320 | } 321 | 322 | /** 323 | * @brief Hardware interface for commanding (forwarding) joint-based trajectories 324 | */ 325 | using JointTrajectoryInterface = TrajectoryInterface; 326 | 327 | /** 328 | * @brief Hardware interface for commanding (forwarding) Cartesian trajectories 329 | */ 330 | using CartesianTrajectoryInterface = TrajectoryInterface; 331 | 332 | } // namespace hardware_interface 333 | -------------------------------------------------------------------------------- /include/pass_through_controllers/pass_through_controllers.hpp: -------------------------------------------------------------------------------- 1 | // -- BEGIN LICENSE BLOCK ---------------------------------------------- 2 | // Copyright 2020 FZI Forschungszentrum Informatik 3 | // Created on behalf of Universal Robots A/S 4 | // 5 | // Licensed under the Apache License, Version 2.0 (the "License"); 6 | // you may not use this file except in compliance with the License. 7 | // You may obtain a copy of the License at 8 | // 9 | // http://www.apache.org/licenses/LICENSE-2.0 10 | // 11 | // Unless required by applicable law or agreed to in writing, software 12 | // distributed under the License is distributed on an "AS IS" BASIS, 13 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | // See the License for the specific language governing permissions and 15 | // limitations under the License. 16 | // -- END LICENSE BLOCK ------------------------------------------------ 17 | 18 | //----------------------------------------------------------------------------- 19 | /*!\file pass_through_controllers.hpp 20 | * 21 | * \author Stefan Scherzinger 22 | * \date 2020/07/16 23 | * 24 | */ 25 | //----------------------------------------------------------------------------- 26 | 27 | // Project 28 | #include 29 | #include 30 | 31 | // Other 32 | #include "ros/duration.h" 33 | #include "ros/timer.h" 34 | #include "speed_scaling_interface/speed_scaling_interface.h" 35 | #include 36 | #include 37 | 38 | namespace trajectory_controllers 39 | { 40 | template 41 | bool PassThroughController::init(hardware_interface::RobotHW* hw, ros::NodeHandle& root_nh, 42 | ros::NodeHandle& controller_nh) 43 | { 44 | // Get names of joints from the parameter server 45 | if (!controller_nh.getParam("joints", joint_names_)) 46 | { 47 | ROS_ERROR_STREAM("Failed to load " << controller_nh.getNamespace() << "/joints from parameter server"); 48 | return false; 49 | } 50 | 51 | // Availability checked by MultiInterfaceController 52 | trajectory_interface_ = hw->get(); 53 | if (!trajectory_interface_) 54 | { 55 | ROS_ERROR_STREAM(controller_nh.getNamespace() << ": No suitable trajectory interface found."); 56 | return false; 57 | } 58 | 59 | trajectory_interface_->setResources(joint_names_); 60 | 61 | // Use speed scaling interface if available (optional). 62 | auto speed_scaling_interface = hw->get(); 63 | if (!speed_scaling_interface) 64 | { 65 | ROS_INFO_STREAM(controller_nh.getNamespace() << ": Your RobotHW seems not to provide speed scaling. Starting " 66 | "without this feature."); 67 | speed_scaling_ = nullptr; 68 | } 69 | else 70 | { 71 | speed_scaling_ = std::make_unique(speed_scaling_interface->getHandle("speed" 72 | "_scal" 73 | "ing_" 74 | "facto" 75 | "r")); 76 | } 77 | 78 | // Action server 79 | action_server_.reset(new actionlib::SimpleActionServer( 80 | controller_nh, 81 | std::is_same::value ? "follow_joint_" 82 | "trajectory" : 83 | "follow_cartesian_" 84 | "trajectory", 85 | std::bind(&PassThroughController::executeCB, this, std::placeholders::_1), false)); 86 | 87 | // This is the cleanest method to notify the vendor robot driver of 88 | // preempted requests. 89 | action_server_->registerPreemptCallback(std::bind(&PassThroughController::preemptCB, this)); 90 | 91 | // Register callback for when hardware finishes (prematurely) 92 | trajectory_interface_->registerDoneCallback(std::bind(&PassThroughController::doneCB, this, std::placeholders::_1)); 93 | 94 | action_server_->start(); 95 | 96 | return true; 97 | } 98 | 99 | template 100 | void PassThroughController::starting(const ros::Time& time) 101 | { 102 | done_ = true; // wait with update() until the next goal comes in. 103 | } 104 | 105 | template 106 | void PassThroughController::stopping(const ros::Time& time) 107 | { 108 | if (action_server_->isActive()) 109 | { 110 | // Stop trajectory interpolation on the robot 111 | trajectory_interface_->setCancel(); 112 | 113 | typename Base::FollowTrajectoryResult result; 114 | result.error_string = "Controller stopped."; 115 | result.error_code = Base::FollowTrajectoryResult::PATH_TOLERANCE_VIOLATED; 116 | action_server_->setAborted(result); 117 | done_ = true; 118 | } 119 | } 120 | 121 | template 122 | void PassThroughController::update(const ros::Time& time, const ros::Duration& period) 123 | { 124 | if (action_server_->isActive() && !done_) 125 | { 126 | // Measure action duration and apply speed scaling if available. 127 | const double factor = (speed_scaling_) ? *speed_scaling_->getScalingFactor() : 1.0; 128 | action_duration_.current += period * factor; 129 | 130 | typename Base::TrajectoryFeedback f = trajectory_interface_->getFeedback(); 131 | action_server_->publishFeedback(f); 132 | 133 | // Check tolerances on each call and set terminal conditions for the 134 | // action server if special criteria are met. 135 | // Also set the done_ flag once that happens. 136 | monitorExecution(f); 137 | 138 | // Time is up. 139 | if (action_duration_.current >= action_duration_.target) 140 | { 141 | if (!done_) 142 | { 143 | ROS_WARN_THROTTLE(3, "The trajectory should be finished by now. " 144 | "Something might be wrong with the robot. " 145 | "You might want to cancel this goal."); 146 | } 147 | } 148 | } 149 | } 150 | 151 | template 152 | void PassThroughController::executeCB(const typename Base::GoalConstPtr& goal) 153 | { 154 | // Upon entering this callback, the simple action server has already 155 | // preempted the previously active goal (if any) and has accepted the new goal. 156 | 157 | if (!this->isRunning()) 158 | { 159 | ROS_ERROR("Can't accept new action goals. Controller is not running."); 160 | typename Base::FollowTrajectoryResult result; 161 | result.error_code = Base::FollowTrajectoryResult::INVALID_GOAL; 162 | action_server_->setAborted(result); 163 | return; 164 | } 165 | 166 | // Only allow correct tolerances if given 167 | if (!isValid(goal)) 168 | { 169 | return; 170 | } 171 | path_tolerances_ = goal->path_tolerance; 172 | goal_tolerances_ = goal->goal_tolerance; 173 | 174 | // Notify the vendor robot control. 175 | if (!trajectory_interface_->setGoal(*goal)) 176 | { 177 | ROS_ERROR("Trajectory goal is invalid."); 178 | typename Base::FollowTrajectoryResult result; 179 | result.error_code = Base::FollowTrajectoryResult::INVALID_GOAL; 180 | action_server_->setAborted(result); 181 | return; 182 | } 183 | 184 | // Time keeping 185 | action_duration_.current = ros::Duration(0.0); 186 | action_duration_.target = goal->trajectory.points.back().time_from_start + goal->goal_time_tolerance; 187 | 188 | done_ = false; 189 | while (!done_) 190 | { 191 | ros::Duration(0.01).sleep(); 192 | } 193 | 194 | // When done, the action server is in one of the three states: 195 | // 1) succeeded: managed in doneCB() 196 | // 2) aborted: managed in update() or in doneCB() 197 | // 3) preempted: managed in preemptCB() 198 | } 199 | 200 | template 201 | void PassThroughController::preemptCB() 202 | { 203 | if (action_server_->isActive()) 204 | { 205 | // Notify the vendor robot control. 206 | trajectory_interface_->setCancel(); 207 | } 208 | } 209 | 210 | template 211 | void PassThroughController::monitorExecution(const typename Base::TrajectoryFeedback& feedback) 212 | { 213 | // Preempt if any of the joints exceeds its path tolerance 214 | if (!withinTolerances(feedback.error, path_tolerances_)) 215 | { 216 | trajectory_interface_->setCancel(); 217 | } 218 | } 219 | 220 | template <> 221 | bool PassThroughController::withinTolerances(const TrajectoryPoint& error, 222 | const Tolerance& tolerances) 223 | { 224 | // Check each user-given tolerance field individually. 225 | // Fail if either the tolerance is exceeded or if the robot driver does not 226 | // provide semantically correct data (conservative fail). 227 | for (size_t i = 0; i < tolerances.size(); ++i) 228 | { 229 | if (tolerances[i].position > 0.0) 230 | { 231 | if (error.positions.size() == tolerances.size()) 232 | { 233 | return std::abs(error.positions[i]) <= tolerances[i].position; 234 | } 235 | ROS_WARN("Position tolerances specified, but not fully supported by the driver implementation."); 236 | return false; 237 | } 238 | 239 | if (tolerances[i].velocity > 0.0) 240 | { 241 | if (error.velocities.size() == tolerances.size()) 242 | { 243 | return std::abs(error.velocities[i]) <= tolerances[i].velocity; 244 | } 245 | ROS_WARN("Velocity tolerances specified, but not fully supported by the driver implementation."); 246 | return false; 247 | } 248 | 249 | if (tolerances[i].acceleration > 0.0) 250 | { 251 | if (error.accelerations.size() == tolerances.size()) 252 | { 253 | return std::abs(error.accelerations[i]) <= tolerances[i].acceleration; 254 | } 255 | ROS_WARN("Acceleration tolerances specified, but not fully supported by the driver implementation."); 256 | return false; 257 | } 258 | } 259 | 260 | // Every error is ok for uninitialized tolerances 261 | return true; 262 | } 263 | 264 | template <> 265 | bool PassThroughController::withinTolerances( 266 | const typename Base::TrajectoryPoint& error, const typename Base::Tolerance& tolerances) 267 | { 268 | // Every error is ok for uninitialized tolerances 269 | Base::Tolerance uninitialized; 270 | std::stringstream str_1; 271 | std::stringstream str_2; 272 | str_1 << tolerances; 273 | str_2 << uninitialized; 274 | 275 | if (str_1.str() == str_2.str()) 276 | { 277 | return true; 278 | } 279 | 280 | auto not_within_limits = [](auto& a, auto& b) { return a.x > b.x || a.y > b.y || a.z > b.z; }; 281 | 282 | // Check each individual dimension separately. 283 | if (not_within_limits(error.pose.position, tolerances.position_error) || 284 | not_within_limits(error.pose.orientation, tolerances.orientation_error) || 285 | not_within_limits(error.twist.linear, tolerances.twist_error.linear) || 286 | not_within_limits(error.twist.angular, tolerances.twist_error.angular) || 287 | not_within_limits(error.acceleration.linear, tolerances.acceleration_error.linear) || 288 | not_within_limits(error.acceleration.angular, tolerances.acceleration_error.angular)) 289 | { 290 | return false; 291 | } 292 | 293 | return true; 294 | } 295 | 296 | template <> 297 | bool PassThroughController::isValid( 298 | const typename Base::GoalConstPtr& goal) 299 | { 300 | // If tolerances are given, they must be given for all joints. 301 | if ((!goal->path_tolerance.empty() && goal->path_tolerance.size() != joint_names_.size()) || 302 | (!goal->goal_tolerance.empty() && goal->goal_tolerance.size() != joint_names_.size())) 303 | { 304 | ROS_ERROR("Given tolerances must match the number of joints"); 305 | typename Base::FollowTrajectoryResult result; 306 | result.error_code = Base::FollowTrajectoryResult::INVALID_GOAL; 307 | action_server_->setAborted(result); 308 | return false; 309 | } 310 | return true; 311 | } 312 | 313 | template <> 314 | bool PassThroughController::isValid( 315 | const typename Base::GoalConstPtr& goal) 316 | { 317 | // No plausibility check required. All possible user inputs seem fine for now. 318 | return true; 319 | } 320 | 321 | template 322 | void PassThroughController::doneCB(const hardware_interface::ExecutionState& state) 323 | { 324 | typename Base::FollowTrajectoryResult result; 325 | 326 | if (!action_server_->isActive()) 327 | { 328 | return; 329 | } 330 | 331 | switch (state) 332 | { 333 | case hardware_interface::ExecutionState::ABORTED: { 334 | result.error_string = "Trajectory aborted by the robot. Something unexpected happened."; 335 | result.error_code = Base::FollowTrajectoryResult::PATH_TOLERANCE_VIOLATED; 336 | action_server_->setAborted(result); 337 | } 338 | break; 339 | 340 | case hardware_interface::ExecutionState::PREEMPTED: { 341 | result.error_string = "Trajectory preempted. Possible reasons: user request | path tolerances fail."; 342 | result.error_code = Base::FollowTrajectoryResult::PATH_TOLERANCE_VIOLATED; 343 | action_server_->setPreempted(result); 344 | } 345 | break; 346 | 347 | case hardware_interface::ExecutionState::SUCCESS: { 348 | // Check if we meet the goal tolerances. 349 | // The most recent feedback gets us the current state. 350 | typename Base::TrajectoryPoint p = trajectory_interface_->getFeedback().error; 351 | if (!withinTolerances(p, goal_tolerances_)) 352 | { 353 | result.error_string = "Trajectory finished execution but failed goal tolerances"; 354 | result.error_code = Base::FollowTrajectoryResult::GOAL_TOLERANCE_VIOLATED; 355 | action_server_->setAborted(result); 356 | } 357 | else 358 | { 359 | result.error_string = "Trajectory execution successful"; 360 | result.error_code = Base::FollowTrajectoryResult::SUCCESSFUL; 361 | action_server_->setSucceeded(result); 362 | } 363 | } 364 | break; 365 | 366 | default: 367 | result.error_string = "Trajectory finished in unknown state."; 368 | result.error_code = Base::FollowTrajectoryResult::PATH_TOLERANCE_VIOLATED; 369 | action_server_->setAborted(result); 370 | break; 371 | } 372 | 373 | done_ = true; 374 | } 375 | 376 | } // namespace trajectory_controllers 377 | --------------------------------------------------------------------------------