├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake └── find_ros1_package.cmake ├── include └── action_bridge │ └── action_bridge.hpp ├── package.xml └── src ├── action_bridge_fibonacci.cpp └── action_bridge_follow_joint_trajectory.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(action_bridge) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | find_package(rclcpp REQUIRED) 21 | find_package(rclcpp_action REQUIRED) 22 | 23 | find_package(example_interfaces REQUIRED) 24 | find_package(control_msgs REQUIRED) 25 | 26 | # find ROS 1 packages 27 | include(cmake/find_ros1_package.cmake) 28 | 29 | find_package(PkgConfig REQUIRED) 30 | 31 | find_ros1_package(roscpp REQUIRED) 32 | find_ros1_package(actionlib REQUIRED) 33 | 34 | find_ros1_package(actionlib_tutorials REQUIRED) 35 | find_ros1_package(control_msgs REQUIRED) 36 | 37 | 38 | ament_export_include_directories(include) 39 | 40 | include_directories(include) 41 | 42 | add_executable(action_bridge_follow_joint_trajectory_node 43 | "src/action_bridge_follow_joint_trajectory.cpp" 44 | ) 45 | ament_target_dependencies(action_bridge_follow_joint_trajectory_node 46 | "ros1_roscpp" "ros1_actionlib" "ros1_control_msgs" 47 | "rclcpp" "rclcpp_action" "control_msgs" 48 | ) 49 | 50 | add_executable(action_bridge_fibonacci_node 51 | "src/action_bridge_fibonacci.cpp" 52 | ) 53 | ament_target_dependencies(action_bridge_fibonacci_node 54 | "ros1_roscpp" "ros1_actionlib" "ros1_actionlib_tutorials" 55 | "rclcpp" "rclcpp_action" "example_interfaces" 56 | ) 57 | 58 | install(TARGETS action_bridge_follow_joint_trajectory_node action_bridge_fibonacci_node 59 | DESTINATION lib/${PROJECT_NAME} 60 | ) 61 | 62 | if(BUILD_TESTING) 63 | find_package(ament_lint_auto REQUIRED) 64 | # the following line skips the linter which checks for copyrights 65 | # remove the line when a copyright and license is present in all source files 66 | set(ament_cmake_copyright_FOUND TRUE) 67 | # the following line skips cpplint (only works in a git repo) 68 | # remove the line when this package is a git repo 69 | set(ament_cmake_cpplint_FOUND TRUE) 70 | ament_lint_auto_find_test_dependencies() 71 | endif() 72 | 73 | ament_package() 74 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | A package to bridge actions between ROS1 and ROS2. 2 | 3 | Please clone `dashing-devel` branch in case you are working with ROS Dashing. 4 | 5 | **NOTE:** 6 | - Currently supports forwarding goals from ROS1 (melodic) action client to ROS2 (crystal) action server 7 | - As an example, implemented interfaces for the action bridge for FibonacciAction 8 | and FollowJointTrajectoryAction 9 | 10 | **Prerequisites:** 11 | 12 | (*rosdep does not work properly with mixed ROS1 and ROS2 dependencies*) 13 | 14 | ``` 15 | sudo apt install ros-melodic-actionlib ros-melodic-actionlib-tutorials ros-melodic-control-msgs ros-melodic-roscpp ros-crystal-control-msgs ros-crystal-example-interfaces ros-crystal-rclcpp ros-crystal-rclcpp-action 16 | ``` 17 | 18 | **How to build:** 19 | 20 | Clone the repository in the `src` folder of your ROS2 workspace. 21 | ``` 22 | git clone git@github.com:ipa-hsd/action_bridge.git 23 | ``` 24 | Since the package provides a bridge for `FollowJointTrajectoryAction`, which is part of the `control_msgs`, clone the package modified for ROS2. 25 | ``` 26 | git clone -b crystal-devel git@github.com:ros-controls/control_msgs.git 27 | ``` 28 | 29 | Since `action_bridge` package depends on both ROS1 and ROS2, source both workspaces. 30 | ``` 31 | source /opt/ros/melodic/local_setup.bash 32 | source /opt/ros/crystal/local_setup.bash 33 | colcon build 34 | ``` 35 | Now you are ready to run the `action_bridge`! 36 | Source this workspace to use the executeables built in the previous step. 37 | ``` 38 | source /install/local_setup.bash 39 | ``` 40 | 2 example executables are available: 41 | - `action_bridge_fibonacci_node` and 42 | - `action_bridge_follow_joint_trajectory_node` 43 | You can start one of these nodes in the following manner: 44 | ``` 45 | ros2 run action_bridge action_bridge_fibonacci_node 46 | ``` 47 | OR 48 | ``` 49 | ros2 run action_bridge action_bridge_follow_joint_trajectory_node 50 | ``` 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /cmake/find_ros1_package.cmake: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | macro(find_ros1_package name) 16 | cmake_parse_arguments(_ARG "REQUIRED" "" "" ${ARGN}) 17 | if(_ARG_UNPARSED_ARGUMENTS) 18 | message(FATAL_ERROR 19 | "find_ros1_package() called with unused arguments: " 20 | "${ARG_UNPARSED_ARGUMENTS}") 21 | endif() 22 | 23 | # the error message of pkg_check_modules for not found required modules 24 | # doesn't include the module name which is not helpful 25 | pkg_check_modules(ros1_${name} ${name}) 26 | if(NOT ros1_${name}_FOUND) 27 | if(_ARG_REQUIRED) 28 | message(FATAL_ERROR 29 | "find_ros1_package() failed to find '${name}' using " 30 | "pkg_check_modules()") 31 | endif() 32 | else() 33 | set(_libraries "${ros1_${name}_LIBRARIES}") 34 | # Prior to catkin 0.7.7, dependent libraries in the pkg-config file were always generated 35 | # in the form "-l:/absolute/path/to/library". Because of the leading "-l", this made 36 | # them show up in ${ros1_${name}_LIBRARIES}. catkin 0.7.7 and later has changed this to 37 | # just be an absolute path (of the form "/absolute/path/to/library"), so we now have to 38 | # look through the LDFLAGS_OTHER and add those to the libraries that we need to link with. 39 | foreach(_flag ${ros1_${name}_LDFLAGS_OTHER}) 40 | if(IS_ABSOLUTE ${_flag}) 41 | list(APPEND _libraries "${_flag}") 42 | endif() 43 | endforeach() 44 | set(_library_dirs "${ros1_${name}_LIBRARY_DIRS}") 45 | set(ros1_${name}_LIBRARIES "") 46 | set(ros1_${name}_LIBRARY_DIRS "") 47 | foreach(_library ${_libraries}) 48 | string(SUBSTRING "${_library}" 0 1 _prefix) 49 | if("${_prefix} " STREQUAL ": ") 50 | string(SUBSTRING "${_library}" 1 -1 _rest) 51 | list(APPEND ros1_${name}_LIBRARIES ${_rest}) 52 | elseif(IS_ABSOLUTE ${_library}) 53 | list(APPEND ros1_${name}_LIBRARIES ${_library}) 54 | else() 55 | set(_lib "${_library}-NOTFOUND") 56 | set(_lib_path "") 57 | # since the path where the library is found is not returned 58 | # this has to be done for each path separately 59 | foreach(_path ${_library_dirs}) 60 | find_library(_lib ${_library} 61 | PATHS ${_path} 62 | NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH) 63 | if(_lib) 64 | set(_lib_path ${_path}) 65 | break() 66 | endif() 67 | endforeach() 68 | if(_lib) 69 | list(APPEND ros1_${name}_LIBRARIES ${_lib}) 70 | list_append_unique(ros1_${name}_LIBRARY_DIRS ${_lib_path}) 71 | else() 72 | # as a fall back try to search globally 73 | find_library(_lib ${_library}) 74 | if(NOT _lib) 75 | message(FATAL_ERROR "pkg-config module '${name}' failed to find library '${_library}'") 76 | endif() 77 | list(APPEND ros1_${name}_LIBRARIES ${_lib}) 78 | endif() 79 | endif() 80 | endforeach() 81 | endif() 82 | endmacro() 83 | -------------------------------------------------------------------------------- /include/action_bridge/action_bridge.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Fraunhofer IPA 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef ACTION_BRIDGE__ACTION_BRIDGE_HPP_ 16 | #define ACTION_BRIDGE__ACTION_BRIDGE_HPP_ 17 | 18 | #ifdef __clang__ 19 | # pragma clang diagnostic push 20 | # pragma clang diagnostic ignored "-Wunused-parameter" 21 | #endif 22 | #include 23 | #include 24 | #ifdef __clang__ 25 | # pragma clang diagnostic pop 26 | #endif 27 | 28 | // include ROS 2 29 | #include "rclcpp/rclcpp.hpp" 30 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | template 40 | class ActionBridge 41 | { 42 | public: 43 | using ROS1GoalHandle = typename actionlib::ActionServer::GoalHandle; 44 | ActionBridge( 45 | ros::NodeHandle ros1_node, 46 | rclcpp::Node::SharedPtr ros2_node, 47 | const std::string action_name) 48 | : ros1_node_(ros1_node), ros2_node_(ros2_node), 49 | server_(ros1_node, action_name, 50 | std::bind(&ActionBridge::goal_cb, this, std::placeholders::_1), 51 | std::bind(&ActionBridge::cancel_cb, this, std::placeholders::_1), 52 | false) 53 | { 54 | server_.start(); 55 | client_ = rclcpp_action::create_client(ros2_node, action_name); 56 | } 57 | 58 | void cancel_cb(ROS1GoalHandle gh1) 59 | { 60 | // try to find goal and cancel it 61 | std::lock_guard lock(mutex_); 62 | auto it = goals_.find(gh1.getGoalID().id); 63 | if (it != goals_.end()) { 64 | std::thread([handler = it->second]() mutable { 65 | handler->cancel(); 66 | }).detach(); 67 | } 68 | } 69 | void goal_cb(ROS1GoalHandle gh1) 70 | { 71 | const std::string goal_id = gh1.getGoalID().id; 72 | 73 | // create a new handler for the goal 74 | std::shared_ptr handler; 75 | handler.reset(new GoalHandler(gh1, client_)); 76 | std::lock_guard lock(mutex_); 77 | goals_.insert(std::make_pair(goal_id, handler)); 78 | 79 | RCLCPP_INFO(ros2_node_->get_logger(), "Sending goal"); 80 | std::thread([handler, goal_id, this]() mutable { 81 | // execute the goal remotely 82 | handler->handle(); 83 | 84 | // clean-up 85 | std::lock_guard lock(mutex_); 86 | goals_.erase(goal_id); 87 | }).detach(); 88 | } 89 | 90 | static int main(const std::string & action_name, int argc, char * argv[]) 91 | { 92 | std::string node_name = "action_bridge_" + action_name; 93 | std::replace(node_name.begin(), node_name.end(), '/', '_'); 94 | // ROS 1 node 95 | ros::init(argc, argv, node_name); 96 | ros::NodeHandle ros1_node; 97 | 98 | // ROS 2 node 99 | rclcpp::init(argc, argv); 100 | auto ros2_node = rclcpp::Node::make_shared(node_name); 101 | 102 | ActionBridge action_bridge(ros1_node, ros2_node, action_name); 103 | 104 | // // ROS 1 asynchronous spinner 105 | ros::AsyncSpinner async_spinner(0); 106 | async_spinner.start(); 107 | 108 | rclcpp::spin(ros2_node); 109 | ros::shutdown(); 110 | return 0; 111 | } 112 | 113 | private: 114 | using ROS1Goal = typename actionlib::ActionServer::Goal; 115 | using ROS1Feedback = typename actionlib::ActionServer::Feedback; 116 | using ROS1Result = typename actionlib::ActionServer::Result; 117 | using ROS2Goal = typename ROS2_T::Goal; 118 | using ROS2Feedback = typename ROS2_T::Feedback; 119 | using ROS2Result = typename ROS2_T::Result; 120 | using ROS2GoalHandle = typename rclcpp_action::ClientGoalHandle::SharedPtr; 121 | using ROS2ClientSharedPtr = typename rclcpp_action::Client::SharedPtr; 122 | 123 | class GoalHandler 124 | { 125 | public: 126 | void cancel() 127 | { 128 | std::lock_guard lock(mutex_); 129 | canceled_ = true; 130 | if (gh2_) { // cancel goal if possible 131 | auto fut = client_->async_cancel_goal(gh2_); 132 | } 133 | } 134 | void handle() 135 | { 136 | auto goal1 = gh1_.getGoal(); 137 | ROS2Goal goal2; 138 | translate_goal_1_to_2(*gh1_.getGoal(), goal2); 139 | 140 | if (!client_->wait_for_action_server(std::chrono::seconds(1))) { 141 | std::cout << "Action server not available after waiting" << std::endl; 142 | gh1_.setRejected(); 143 | return; 144 | } 145 | 146 | // send goal to ROS2 server, set-up feedback 147 | auto gh2_future = client_->async_send_goal(goal2, 148 | [this](ROS2GoalHandle, auto feedback2) { 149 | ROS1Feedback feedback1; 150 | translate_feedback_2_to_1(feedback1, *feedback2); 151 | gh1_.publishFeedback(feedback1); 152 | } 153 | ); 154 | 155 | auto goal_handle = gh2_future.get(); 156 | if (!goal_handle) { 157 | gh1_.setRejected(); // goal was not accepted by remote server 158 | return; 159 | } 160 | gh1_.setAccepted(); 161 | 162 | { 163 | std::lock_guard lock(mutex_); 164 | gh2_ = goal_handle; 165 | 166 | if (canceled_) { // cancel was called in between 167 | auto fut = client_->async_cancel_goal(gh2_); 168 | } 169 | } 170 | 171 | // wait for result and forward it to the ROS1 client 172 | auto res2 = client_->async_get_result(gh2_).get(); 173 | 174 | ROS1Result res1; 175 | translate_result_2_to_1(res1, *res2.response); 176 | 177 | std::lock_guard lock(mutex_); 178 | if (res2.code == rclcpp_action::ResultCode::SUCCEEDED) { 179 | gh1_.setSucceeded(res1); 180 | } else if (res2.code == rclcpp_action::ResultCode::CANCELED) { 181 | gh1_.setCanceled(res1); 182 | } else { 183 | gh1_.setAborted(res1); 184 | } 185 | } 186 | 187 | GoalHandler(ROS1GoalHandle & gh1, ROS2ClientSharedPtr & client) 188 | : gh1_(gh1), gh2_(nullptr), client_(client), canceled_(false) {} 189 | 190 | private: 191 | ROS1GoalHandle gh1_; 192 | ROS2GoalHandle gh2_; 193 | ROS2ClientSharedPtr client_; 194 | bool canceled_; // cancel was called 195 | std::mutex mutex_; 196 | 197 | }; 198 | 199 | ros::NodeHandle ros1_node_; 200 | rclcpp::Node::SharedPtr ros2_node_; 201 | 202 | actionlib::ActionServer server_; 203 | ROS2ClientSharedPtr client_; 204 | 205 | std::mutex mutex_; 206 | std::map> goals_; 207 | 208 | static void translate_goal_1_to_2(const ROS1Goal &, ROS2Goal &); 209 | static void translate_result_2_to_1(ROS1Result &, const ROS2Result &); 210 | static void translate_feedback_2_to_1(ROS1Feedback &, const ROS2Feedback &); 211 | }; 212 | 213 | #endif // ACTION_BRIDGE__ACTION_BRIDGE_HPP_ 214 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | action_bridge 5 | 0.1.0 6 | Example action bridges for FibonacciAction and FollowJointTrajectoryAction 7 | 8 | Harshavardhan Deshpande 9 | 10 | Apache License 2.0 11 | 12 | Harshavardhan Deshpande 13 | Mathias Lüdtke 14 | 15 | ament_cmake 16 | 17 | 18 | control_msgs 19 | 20 | ament_lint_auto 21 | ament_lint_common 22 | 23 | 24 | ament_cmake 25 | 26 | 27 | -------------------------------------------------------------------------------- /src/action_bridge_fibonacci.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Fraunhofer IPA 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #ifdef __clang__ 18 | # pragma clang diagnostic push 19 | # pragma clang diagnostic ignored "-Wunused-parameter" 20 | #endif 21 | #include 22 | #ifdef __clang__ 23 | # pragma clang diagnostic pop 24 | #endif 25 | 26 | // include ROS 2 27 | #include 28 | 29 | using FibonacciActionBridge = ActionBridge; 31 | 32 | template<> 33 | void FibonacciActionBridge::translate_goal_1_to_2(const ROS1Goal & goal1, ROS2Goal & goal2) 34 | { 35 | goal2.order = goal1.order; 36 | } 37 | 38 | template<> 39 | void FibonacciActionBridge::translate_result_2_to_1( 40 | ROS1Result & result1, 41 | const ROS2Result & result2) 42 | { 43 | result1.sequence = result2.sequence; 44 | } 45 | 46 | template<> 47 | void FibonacciActionBridge::translate_feedback_2_to_1( 48 | ROS1Feedback & feedback1, 49 | const ROS2Feedback & feedback2) 50 | { 51 | feedback1.sequence = feedback2.sequence; 52 | } 53 | 54 | int main(int argc, char * argv[]) 55 | { 56 | return FibonacciActionBridge::main("fibonacci", argc, argv); 57 | } 58 | -------------------------------------------------------------------------------- /src/action_bridge_follow_joint_trajectory.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Fraunhofer IPA 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | // include ROS 1 18 | #ifdef __clang__ 19 | # pragma clang diagnostic push 20 | # pragma clang diagnostic ignored "-Wunused-parameter" 21 | #endif 22 | #include 23 | #ifdef __clang__ 24 | # pragma clang diagnostic pop 25 | #endif 26 | 27 | // include ROS 2 28 | #include "control_msgs/action/follow_joint_trajectory.hpp" 29 | 30 | template 31 | static void copy_point(const T1 & pt1, T2 & pt2) 32 | { 33 | pt2.positions = pt1.positions; 34 | pt2.velocities = pt1.velocities; 35 | pt2.accelerations = pt1.accelerations; 36 | } 37 | 38 | template 39 | static void copy_tolerance(const T1 & tolerance1, T2 & tolerance2) 40 | { 41 | tolerance2.name = tolerance1.name; 42 | tolerance2.position = tolerance1.position; 43 | tolerance2.velocity = tolerance1.velocity; 44 | tolerance2.acceleration = tolerance1.acceleration; 45 | } 46 | 47 | template 48 | static void copy_tolerances(const T1 & t1, T2 & t2) 49 | { 50 | const size_t num = t1.size(); 51 | t2.resize(num); 52 | for (size_t i = 0; i < num; ++i) { 53 | copy_tolerance(t1[i], t2[i]); 54 | } 55 | } 56 | 57 | static void copy_duration_1_to_2( 58 | const ros::Duration & duration1, 59 | builtin_interfaces::msg::Duration & duration2) 60 | { 61 | duration2.sec = duration1.sec; 62 | duration2.nanosec = duration1.nsec; 63 | } 64 | 65 | using FollowJointTrajectoryActionBridge = ActionBridge; 67 | 68 | template<> 69 | void FollowJointTrajectoryActionBridge::translate_goal_1_to_2( 70 | const ROS1Goal & goal1, 71 | ROS2Goal & goal2) 72 | { 73 | goal2.trajectory.joint_names = goal1.trajectory.joint_names; 74 | const size_t num = goal1.trajectory.points.size(); 75 | goal2.trajectory.points.resize(num); 76 | 77 | for (size_t i = 0; i < num; ++i) { 78 | copy_point(goal1.trajectory.points[i], goal2.trajectory.points[i]); 79 | copy_duration_1_to_2(goal1.trajectory.points[i].time_from_start, 80 | goal2.trajectory.points[i].time_from_start); 81 | } 82 | 83 | copy_tolerances(goal1.path_tolerance, goal2.path_tolerance); 84 | copy_tolerances(goal1.goal_tolerance, goal2.goal_tolerance); 85 | 86 | copy_duration_1_to_2(goal1.goal_time_tolerance, goal2.goal_time_tolerance); 87 | } 88 | 89 | template<> 90 | void FollowJointTrajectoryActionBridge::translate_result_2_to_1( 91 | ROS1Result & result1, 92 | const ROS2Result & result2) 93 | { 94 | result1.error_code = result2.error_code; 95 | result1.error_string = result2.error_string; 96 | } 97 | 98 | template<> 99 | void FollowJointTrajectoryActionBridge::translate_feedback_2_to_1( 100 | ROS1Feedback & feedback1, 101 | const ROS2Feedback & feedback2) 102 | { 103 | feedback1.joint_names = feedback2.joint_names; 104 | copy_point(feedback2.desired, feedback1.desired); 105 | copy_point(feedback2.actual, feedback1.actual); 106 | copy_point(feedback2.error, feedback1.error); 107 | } 108 | 109 | int main(int argc, char * argv[]) 110 | { 111 | return FollowJointTrajectoryActionBridge::main("follow_joint_trajectory", argc, argv); 112 | } 113 | --------------------------------------------------------------------------------