├── .gitignore ├── CMakeLists.txt ├── README.md ├── docs ├── FogROS.gif ├── FogROS_CASE2021_Camera Ready.pdf ├── README.md ├── _config.yml ├── quickstart.md └── tutorial_images │ ├── image1.png │ ├── image2.png │ ├── image3.png │ ├── image4.png │ ├── image5.png │ ├── image6.png │ └── image7.png ├── examples ├── README ├── add_two_ints_client.py ├── add_two_ints_server.py ├── listener.py └── talker.py ├── experiments └── Roscloud experiments.xlsx ├── launch ├── README.md ├── gqcnn.bash ├── gqcnn.launch ├── gqcnn_client.bash ├── gqcnn_client.launch ├── gqcnn_vpc.bash ├── mpt.bash ├── mpt.launch ├── mpt_client.launch ├── openvslam.bash ├── openvslam.launch ├── openvslam_client.bash ├── openvslam_vpc.bash ├── roscloud_gqcnn_docker.launch ├── roscloud_gqcnn_launch.launch ├── roscloud_gqcnn_vpc.launch ├── roscloud_mpt.launch ├── roscloud_openvslam.launch ├── roscloud_openvslam_vpc.launch ├── roscloud_talker.launch ├── roscloud_vpc.launch └── talker.launch ├── package.xml ├── scripts ├── .DS_Store ├── __pycache__ │ └── roscloud_base.cpython-38.pyc ├── roscloud_base.py ├── roscloud_docker.py ├── roscloud_launch.py ├── roscloud_vpc.py └── rosduct_main.py ├── setup.py ├── src └── rosduct │ ├── __init__.py │ ├── __pycache__ │ ├── conversions.cpython-38.pyc │ ├── rosbridge_client.cpython-38.pyc │ └── rosduct_impl.cpython-38.pyc │ ├── conversions.py │ ├── rosbridge_client.py │ └── rosduct_impl.py └── srv ├── AddTwoInts.srv └── beginner_tutorials.srv /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | # built files 3 | **/target 4 | 5 | # emacs 6 | *~ 7 | 8 | # mac related 9 | .DS_Store 10 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(FogROS) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | rosduct 13 | rospy 14 | std_msgs 15 | message_generation 16 | ) 17 | 18 | ## System dependencies are found with CMake's conventions 19 | # find_package(Boost REQUIRED COMPONENTS system) 20 | 21 | catkin_python_setup() 22 | 23 | 24 | 25 | ## Generate messages in the 'msg' folder 26 | # add_message_files( 27 | # FILES 28 | # Message1.msg 29 | # Message2.msg 30 | # ) 31 | 32 | ## Generate services in the 'srv' folder 33 | add_service_files( 34 | FILES 35 | AddTwoInts.srv 36 | beginner_tutorials.srv 37 | ) 38 | 39 | 40 | ## Generate actions in the 'action' folder 41 | # add_action_files( 42 | # FILES 43 | # Action1.action 44 | # Action2.action 45 | # ) 46 | 47 | ## Generate added messages and services with any dependencies listed here 48 | generate_messages( 49 | DEPENDENCIES 50 | std_msgs 51 | ) 52 | 53 | 54 | catkin_package( 55 | # INCLUDE_DIRS include 56 | # LIBRARIES roscloud 57 | # CATKIN_DEPENDS roscpp rosduct rospy std_msgs 58 | # DEPENDS system_lib 59 | ) 60 | 61 | ########### 62 | ## Build ## 63 | ########### 64 | 65 | ## Specify additional locations of header files 66 | ## Your package locations should be listed before other locations 67 | include_directories( 68 | # include 69 | ${catkin_INCLUDE_DIRS} 70 | ) 71 | 72 | ## Declare a C++ library 73 | # add_library(${PROJECT_NAME} 74 | # src/${PROJECT_NAME}/roscloud.cpp 75 | # ) 76 | 77 | ## Add cmake target dependencies of the library 78 | ## as an example, code may need to be generated before libraries 79 | ## either from message generation or dynamic reconfigure 80 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 81 | 82 | ## Declare a C++ executable 83 | ## With catkin_make all packages are built within a single CMake context 84 | ## The recommended prefix ensures that target names across packages don't collide 85 | # add_executable(${PROJECT_NAME}_node src/roscloud_node.cpp) 86 | 87 | ## Rename C++ executable without prefix 88 | ## The above recommended prefix causes long target names, the following renames the 89 | ## target back to the shorter version for ease of user use 90 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 91 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 92 | 93 | ## Add cmake target dependencies of the executable 94 | ## same as for the library above 95 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 96 | 97 | ## Specify libraries to link a library or executable target against 98 | # target_link_libraries(${PROJECT_NAME}_node 99 | # ${catkin_LIBRARIES} 100 | # ) 101 | 102 | ############# 103 | ## Install ## 104 | ############# 105 | 106 | # all install targets should use catkin DESTINATION variables 107 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 108 | 109 | ## Mark executable scripts (Python etc.) for installation 110 | ## in contrast to setup.py, you can choose the destination 111 | # catkin_install_python(PROGRAMS 112 | # scripts/my_python_script 113 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 114 | # ) 115 | 116 | ## Mark executables for installation 117 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 118 | # install(TARGETS ${PROJECT_NAME}_node 119 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 120 | # ) 121 | 122 | ## Mark libraries for installation 123 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 124 | # install(TARGETS ${PROJECT_NAME} 125 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 126 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 127 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 128 | # ) 129 | 130 | ## Mark cpp header files for installation 131 | # install(DIRECTORY include/${PROJECT_NAME}/ 132 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 133 | # FILES_MATCHING PATTERN "*.h" 134 | # PATTERN ".svn" EXCLUDE 135 | # ) 136 | 137 | ## Mark other files for installation (e.g. launch and bag files, etc.) 138 | # install(FILES 139 | # # myfile1 140 | # # myfile2 141 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 142 | # ) 143 | 144 | ## Mark executable scripts (Python etc.) for installation 145 | ## in contrast to setup.py, you can choose the destination 146 | install(PROGRAMS 147 | scripts/roscloud_base.py 148 | scripts/roscloud_launch.py 149 | scripts/roscloud_docker.py 150 | scripts/rosduct_main.py 151 | examples/talker.py 152 | examples/listener.py 153 | examples/add_two_ints_client.py 154 | examples/add_two_ints_server.py 155 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 156 | ) 157 | 158 | ## Mark other files for installation (e.g. launch and bag files, etc.) 159 | foreach (dir launch config) 160 | install(DIRECTORY ${dir}/ 161 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) 162 | endforeach(dir) 163 | 164 | ############# 165 | ## Testing ## 166 | ############# 167 | 168 | ## Add gtest based cpp test target and link libraries 169 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_roscloud.cpp) 170 | # if(TARGET ${PROJECT_NAME}-test) 171 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 172 | # endif() 173 | 174 | ## Add folders to be run by python nosetests 175 | # catkin_add_nosetests(test) 176 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Note that this repository is for FogROS v1, which has been deprecated. 2 | We are moving all of our development efforts to [FogROS2](https://github.com/berkeleyAutomation/FogROS2) which is based on ROS2 since ROS1 is being discontinued soon. 3 | = 4 | 5 | # FogROS 6 | 7 | By Kaiyuan(Eric) Chen, Yafei Liang, Nikhil Jha, Jeffrey Ichnowski, Michael Danielczuk, Joseph Gonzalez, John Kubiatowicz, Ken Goldberg 8 | 9 | 10 | 11 | ## What is FogROS 12 | 13 | FogROS is a framework that allows existing ROS automation applications to gain access to additional computing resources from commercial cloud-based services. This framework is built on the [Robot Operating System (ROS)](https://www.ros.org/), the de-facto standard for creating robot automation applications and components. With minimal porting effort, FogROS allows researchers to deploy components of their software to the cloud with high transparency. 14 | 15 | FogROS presents a user-friendly framework that allows computationally intensive parts of robotics programs (e.g., GPU, multi-core CPU, FPGA, TPU) to be run in the cloud; in experiments, common robotics algorithms (motion planning, grasp planning, VSLAM) deployed to the cloud via FogROS showed up to 31.5x performance improvement.The authors aim to make cloud-based deployments through FogROS transparent to researchers and roboticists. 16 | 17 | FogROS takes care of the details of setting up cloud computers, securing communication, and running programs in the cloud, allowing roboticists to benefit for the performance gain, allowing researchers to experiment with different deployments or focus on their application.With FogROS, researchers can deploy their software so that other researchers can make use of it with minimal effort and without concern for hardware compatibility--potentially allowing for increased cross-pollination of research ideas and more productive labs. 18 | 19 | ![architecture diagram depicting 4 nodes, one of which is in the cloud with FogROS](https://github.com/BerkeleyAutomation/FogROS/raw/main/docs/FogROS.gif) 20 | 21 | This project will be published in [IEEE International Conference on Automation Science and Engineering (CASE) 2021](https://case2021.sciencesconf.org/). The video can be found on [Youtube](https://www.youtube.com/watch?v=lSZw_Fkpnm0&t=2s), and the paper can be found [Here](https://github.com/BerkeleyAutomation/FogROS/blob/main/docs/FogROS_CASE2021_Camera%20Ready.pdf). 22 | 23 | ## Install 24 | 25 | #### Step 0: Set up ROS 26 | Set up ROS environment and clone this repo to catkin workspace. ```ami-0d255df33d23c5a9d``` in ```US-west-1``` is a good starting point if you don't have a working ROS environment. 27 | 28 | 29 | #### Step 1: Set up the repo's environments 30 | 31 | 1. you need to run ```aws configure``` to configure the AWS API keys. 32 | 2. ```launch/roscloud*.launch``` has plenty of options. You may specify a docker image, or just several ROS nodes. The code for the demos can be found in [Demo Repo](https://github.com/BerkeleyAutomation/fogros-demos). 33 | 34 | #### Step2: Run it 35 | Simply run 36 | 37 | ```` 38 | roslaunch FogROS roscloud_talker.launch 39 | ```` 40 | 41 | for basic pubisher-subscriber example. The EC2 instance will be automatically created, and you will verify the code is working by running 42 | ``` 43 | rostopic list 44 | ``` 45 | on your robot or local machine. The step-by-step tutorial can be found in our [Wiki](https://github.com/BerkeleyAutomation/FogROS/wiki/Running-FogROS-Examples) 46 | 47 | 48 | 49 | ## Release Plan 50 | 51 | Currently, FogROS is still a research prototype. It is going through code and documentation revisions. We are planning to have official release by mid-September. 52 | 53 | 54 | 55 | ## Contact Us 56 | 57 | If there were any concerns/bugs, please post as Github issue. 58 | -------------------------------------------------------------------------------- /docs/FogROS.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BerkeleyAutomation/FogROS/6c8cd4572b96bb3a8139321e53d1bbb193b907a2/docs/FogROS.gif -------------------------------------------------------------------------------- /docs/FogROS_CASE2021_Camera Ready.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BerkeleyAutomation/FogROS/6c8cd4572b96bb3a8139321e53d1bbb193b907a2/docs/FogROS_CASE2021_Camera Ready.pdf -------------------------------------------------------------------------------- /docs/README.md: -------------------------------------------------------------------------------- 1 | # FogROS: An Adaptive Framework for Automating Fog Robotics Deployment 2 | 3 | FogROS is a framework that allows existing ROS automation applications to gain access to additional computing resources from commercial cloud-based services. This framework is built on the [Robot Operating System (ROS)](https://www.ros.org/), the de-facto standard for creating robot automation applications and components. With minimal porting effort, FogROS allows researchers to deploy components of their software to the cloud with high transparency. 4 | 5 | FogROS presents a user-friendly framework that allows computationally intensive parts of robotics programs (e.g., GPU, multi-core CPU, FPGA, TPU) to be run in the cloud; in experiments, common robotics algorithms (motion planning, grasp planning, VSLAM) deployed to the cloud via FogROS showed up to 31.5x performance improvement.The authors aim to make cloud-based deployments through FogROS transparent to researchers and roboticists. 6 | 7 | FogROS takes care of the details of setting up cloud computers, securing communication, and running programs in the cloud, allowing roboticists to benefit for the performance gain, allowing researchers to experiment with different deployments or focus on their application.With FogROS, researchers can deploy their software so that other researchers can make use of it with minimal effort and without concern for hardware compatibility--potentially allowing for increased cross-pollination of research ideas and more productive labs. 8 | 9 | ![architecture diagram depicting 4 nodes, one of which is in the cloud with FogROS](https://github.com/BerkeleyAutomation/FogROS/raw/main/docs/FogROS.gif) 10 | 11 | This project will be published in [IEEE International Conference on Automation Science and Engineering (CASE) 2021](https://case2021.sciencesconf.org/). The video can be found on [Youtube](https://www.youtube.com/watch?v=lSZw_Fkpnm0&t=2s), and the paper can be found [Here](https://github.com/BerkeleyAutomation/FogROS/blob/main/docs/FogROS_CASE2021_Camera%20Ready.pdf). 12 | -------------------------------------------------------------------------------- /docs/_config.yml: -------------------------------------------------------------------------------- 1 | title: "FogROS" 2 | remote_theme: pmarsceill/just-the-docs 3 | color_scheme: "light" 4 | search_enabled: true 5 | theme: jekyll-theme-slate -------------------------------------------------------------------------------- /docs/quickstart.md: -------------------------------------------------------------------------------- 1 | --- 2 | title: Quickstart 3 | --- 4 | 5 | # Quickstart 6 | 7 | TODO: Write quickstart guide... 8 | -------------------------------------------------------------------------------- /docs/tutorial_images/image1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BerkeleyAutomation/FogROS/6c8cd4572b96bb3a8139321e53d1bbb193b907a2/docs/tutorial_images/image1.png -------------------------------------------------------------------------------- /docs/tutorial_images/image2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BerkeleyAutomation/FogROS/6c8cd4572b96bb3a8139321e53d1bbb193b907a2/docs/tutorial_images/image2.png -------------------------------------------------------------------------------- /docs/tutorial_images/image3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BerkeleyAutomation/FogROS/6c8cd4572b96bb3a8139321e53d1bbb193b907a2/docs/tutorial_images/image3.png -------------------------------------------------------------------------------- /docs/tutorial_images/image4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BerkeleyAutomation/FogROS/6c8cd4572b96bb3a8139321e53d1bbb193b907a2/docs/tutorial_images/image4.png -------------------------------------------------------------------------------- /docs/tutorial_images/image5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BerkeleyAutomation/FogROS/6c8cd4572b96bb3a8139321e53d1bbb193b907a2/docs/tutorial_images/image5.png -------------------------------------------------------------------------------- /docs/tutorial_images/image6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BerkeleyAutomation/FogROS/6c8cd4572b96bb3a8139321e53d1bbb193b907a2/docs/tutorial_images/image6.png -------------------------------------------------------------------------------- /docs/tutorial_images/image7.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BerkeleyAutomation/FogROS/6c8cd4572b96bb3a8139321e53d1bbb193b907a2/docs/tutorial_images/image7.png -------------------------------------------------------------------------------- /examples/README: -------------------------------------------------------------------------------- 1 | protoc -I=. --python_out=. ./lambda_cmd.proto 2 | -------------------------------------------------------------------------------- /examples/add_two_ints_client.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from __future__ import print_function 4 | 5 | import sys 6 | import rospy 7 | from roscloud.srv import * 8 | 9 | def add_two_ints_client(x, y): 10 | rospy.wait_for_service('add_two_ints') 11 | try: 12 | add_two_ints = rospy.ServiceProxy('add_two_ints', AddTwoInts) 13 | resp1 = add_two_ints(x, y) 14 | return resp1.sum 15 | except rospy.ServiceException as e: 16 | print("Service call failed: %s"%e) 17 | 18 | def usage(): 19 | return "%s [x y]"%sys.argv[0] 20 | 21 | if __name__ == "__main__": 22 | if len(sys.argv) == 3: 23 | x = int(sys.argv[1]) 24 | y = int(sys.argv[2]) 25 | else: 26 | print(usage()) 27 | sys.exit(1) 28 | print("Requesting %s+%s"%(x, y)) 29 | print("%s + %s = %s"%(x, y, add_two_ints_client(x, y))) 30 | -------------------------------------------------------------------------------- /examples/add_two_ints_server.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from __future__ import print_function 4 | 5 | from roscloud.srv import AddTwoInts,AddTwoIntsResponse 6 | import rospy 7 | 8 | def handle_add_two_ints(req): 9 | print("Returning [%s + %s = %s]"%(req.a, req.b, (req.a + req.b))) 10 | return AddTwoIntsResponse(req.a + req.b) 11 | 12 | def add_two_ints_server(): 13 | rospy.init_node('add_two_ints_server') 14 | s = rospy.Service('add_two_ints', AddTwoInts, handle_add_two_ints) 15 | print("Ready to add two ints.") 16 | rospy.spin() 17 | 18 | if __name__ == "__main__": 19 | add_two_ints_server() 20 | -------------------------------------------------------------------------------- /examples/listener.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2008, Willow Garage, Inc. 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of Willow Garage, Inc. nor the names of its 18 | # contributors may be used to endorse or promote products derived 19 | # from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | # 34 | # Revision $Id$ 35 | 36 | ## Simple talker demo that listens to std_msgs/Strings published 37 | ## to the 'chatter' topic 38 | 39 | import rospy 40 | from std_msgs.msg import String 41 | 42 | def callback(data): 43 | rospy.loginfo(rospy.get_caller_id() + 'I heard %s', data.data) 44 | 45 | def listener(): 46 | 47 | # In ROS, nodes are uniquely named. If two nodes with the same 48 | # name are launched, the previous one is kicked off. The 49 | # anonymous=True flag means that rospy will choose a unique 50 | # name for our 'listener' node so that multiple listeners can 51 | # run simultaneously. 52 | rospy.init_node('listener', anonymous=True) 53 | 54 | rospy.Subscriber('chatter', String, callback) 55 | 56 | # spin() simply keeps python from exiting until this node is stopped 57 | rospy.spin() 58 | 59 | if __name__ == '__main__': 60 | listener() 61 | -------------------------------------------------------------------------------- /examples/talker.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2008, Willow Garage, Inc. 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of Willow Garage, Inc. nor the names of its 18 | # contributors may be used to endorse or promote products derived 19 | # from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | # 34 | # Revision $Id$ 35 | 36 | ## Simple talker demo that published std_msgs/Strings messages 37 | ## to the 'chatter' topic 38 | 39 | import rospy 40 | from std_msgs.msg import String 41 | 42 | def talker(): 43 | pub = rospy.Publisher('chatter', String, queue_size=10) 44 | rospy.init_node('talker', anonymous=True) 45 | rate = rospy.Rate(10) # 10hz 46 | while not rospy.is_shutdown(): 47 | hello_str = "hello world %s" % rospy.get_time() 48 | rospy.loginfo(hello_str) 49 | pub.publish(hello_str) 50 | rate.sleep() 51 | 52 | if __name__ == '__main__': 53 | try: 54 | talker() 55 | except rospy.ROSInterruptException: 56 | pass 57 | -------------------------------------------------------------------------------- /experiments/Roscloud experiments.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BerkeleyAutomation/FogROS/6c8cd4572b96bb3a8139321e53d1bbb193b907a2/experiments/Roscloud experiments.xlsx -------------------------------------------------------------------------------- /launch/README.md: -------------------------------------------------------------------------------- 1 | # Launch Scripts 2 | 3 | ### How to use 4 | 5 | We will run ```roscloud_*``` on the client, and the script will automatically upload its counterpart to the cloud. For example, if we run 6 | 7 | ```bash 8 | roslaunch roscloud roscloud_talker.launch 9 | ``` 10 | It will start ```roscloud/talker.py``` node on the cloud instance. 11 | 12 | 13 | ### List of files 14 | The following is a list of files that can be run with ```roslaunch roscloud``` 15 | * **roscloud_talker**.launch: it starts a basic talker node on the cloud, to test whether the traffic is relayed to the robot, one can run ```rosrun roscloud listener.py``` on the robot, or simply use ```rostopic echo /chatter```. 16 | * **roscloud_mpt**.launch: it starts a motion planner template on the cloud 17 | * **roscloud_gqcnn_launch**.launch: it starts a dexnet node on the cloud 18 | * **roscloud_openvslam**.launch: it starts an oepnvslam node on the cloud 19 | * **roscloud_vpc_**: are the counterparts that use vpc 20 | -------------------------------------------------------------------------------- /launch/gqcnn.bash: -------------------------------------------------------------------------------- 1 | 2 | sudo apt install -y docker.io 3 | sudo docker pull keplerc/grasp_image:gpu 4 | #sudo docker run -d --network host --rm keplerc/grasp_image:cpu 5 | 6 | sudo docker run --gpus all -d --network host --rm keplerc/grasp_image:gpu 7 | # sleep 10 8 | # sudo docker run --network host --rm keplerc/grasp_image:gpu roslaunch gqcnn_ros gqcnn_ros_client.launch client_args:=" --camera_intr=/root/catkin_ws/src/gqcnn-ros/data/calib/phoxi.intr --depth_image=/root/catkin_ws/src/gqcnn-ros/data/examples/phoxi_clutter/depth_0.npy" 9 | 10 | -------------------------------------------------------------------------------- /launch/gqcnn.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | automatic_topic_scan: False 5 | rosbridge_ip: ROSBRIDGE_IP_HOLDER 6 | rosbridge_port: 9090 7 | # Topics being published in the robot to expose locally 8 | remote_topics: [ 9 | ['/gqcnn/camera_info', 'sensor_msgs/CameraInfo'], 10 | ['/gqcnn/image', 'sensor_msgs/Image'], 11 | ['/gqcnn/mask', 'sensor_msgs/Image'] 12 | ] 13 | # Topics being published in the local roscore to expose remotely 14 | local_topics: [ 15 | ['/gqcnn/grasp', 'gqcnn_ros/GQCNNGrasp'] 16 | ] 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /launch/gqcnn_client.bash: -------------------------------------------------------------------------------- 1 | 2 | sudo apt install -y docker.io 3 | sudo docker pull keplerc/grasp_image:gpu 4 | #sudo docker run -d --network host --rm keplerc/grasp_image:cpu 5 | 6 | # sudo docker run --gpus all -d --network host --rm keplerc/grasp_image:gpu 7 | # sleep 10 8 | sudo docker run --network host --rm keplerc/grasp_image:gpu roslaunch gqcnn_ros gqcnn_ros_client.launch client_args:=" --camera_intr=/root/catkin_ws/src/gqcnn-ros/data/calib/phoxi.intr --depth_image=/root/catkin_ws/src/gqcnn-ros/data/examples/phoxi_clutter/depth_0.npy" compress:=true 9 | 10 | -------------------------------------------------------------------------------- /launch/gqcnn_client.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /launch/gqcnn_vpc.bash: -------------------------------------------------------------------------------- 1 | 2 | sudo apt install -y docker.io 3 | sudo docker pull keplerc/grasp_image:gpu 4 | #sudo docker run -d --network host --rm keplerc/grasp_image:cpu 5 | 6 | sudo docker run --gpus all --network host --rm keplerc/grasp_image:gpu 7 | # sleep 10 8 | # sudo docker run --network host --rm keplerc/grasp_image:gpu roslaunch gqcnn_ros gqcnn_ros_client.launch client_args:=" --camera_intr=/root/catkin_ws/src/gqcnn-ros/data/calib/phoxi.intr --depth_image=/root/catkin_ws/src/gqcnn-ros/data/examples/phoxi_clutter/depth_0.npy" 9 | 10 | -------------------------------------------------------------------------------- /launch/mpt.bash: -------------------------------------------------------------------------------- 1 | 2 | sudo apt install -y libeigen3-dev libassimp-dev libccd-dev 3 | cd ~ 4 | git clone https://github.com/flexible-collision-library/fcl.git 5 | mkdir fcl/build 6 | cd fcl/build 7 | cmake -DCMAKE_BUILD_TYPE=Release .. 8 | make -j$(nproc) 9 | sudo make install 10 | -------------------------------------------------------------------------------- /launch/mpt.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /launch/mpt_client.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /launch/openvslam.bash: -------------------------------------------------------------------------------- 1 | sudo docker pull debbieliang/ros_open:open_ros_dataset 2 | sudo docker pull debbieliang/ros_open:open_ros_server 3 | 4 | sudo docker run -d --network host --rm -it debbieliang/ros_open:open_ros_server 5 | 6 | # sudo docker run -d --network host --rm -it debbieliang/ros_open:open_ros_dataset roscore 7 | 8 | #run this locally 9 | # sudo docker run --network host --rm -it debbieliang/ros_open:open_ros_dataset /bin/bash -c "source /openvslam/ros/devel/setup.bash && rosrun publisher video -m /openvslam/build/aist_living_lab_1/video.mp4" 10 | 11 | sudo docker run -d --network host --rm -it debbieliang/ros_open:open_ros_log /bin/bash -c "source /openvslam/ros/devel/setup.bash && rosrun openvslam run_slam -v /openvslam/build/orb_vocab/orb_vocab.dbow2 -c /openvslam/build/aist_living_lab_1/config.yaml --map-db map.msg" 12 | 13 | -------------------------------------------------------------------------------- /launch/openvslam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /launch/openvslam_client.bash: -------------------------------------------------------------------------------- 1 | sudo docker pull debbieliang/ros_open:open_ros_dataset 2 | sudo docker pull debbieliang/ros_open:open_ros_server 3 | 4 | sudo docker run -d --network host --rm -it debbieliang/ros_open:open_ros_server 5 | 6 | # sudo docker run -d --network host --rm -it debbieliang/ros_open:open_ros_dataset roscore 7 | 8 | #run this locally 9 | sudo docker run --network host --rm -it debbieliang/ros_open:open_ros_latency /bin/bash -c "source /openvslam/ros/devel/setup.bash && rosrun publisher video -m /openvslam/build/aist_living_lab_1/video.mp4" 10 | 11 | #sudo docker run -d --network host --rm -it debbieliang/ros_open:open_ros_log /bin/bash -c "source /openvslam/ros/devel/setup.bash && rosrun openvslam run_slam -v /openvslam/build/orb_vocab/orb_vocab.dbow2 -c /openvslam/build/aist_living_lab_1/config.yaml --map-db map.msg" 12 | 13 | -------------------------------------------------------------------------------- /launch/openvslam_vpc.bash: -------------------------------------------------------------------------------- 1 | sudo docker pull debbieliang/ros_open:open_ros_dataset 2 | sudo docker pull debbieliang/ros_open:open_ros_server 3 | 4 | sudo docker run -d --network host --rm -it debbieliang/ros_open:open_ros_dataset roscore 5 | 6 | sudo docker run -d --network host --rm -it debbieliang/ros_open:open_ros_server 7 | 8 | #run this locally 9 | # sudo docker run --network host --rm -it debbieliang/ros_open:open_ros_dataset /bin/bash -c "source /openvslam/ros/devel/setup.bash && rosrun publisher video -m /openvslam/build/aist_living_lab_1/video.mp4" 10 | 11 | sudo docker run -d --network host --rm -it debbieliang/ros_open:open_ros_latency /bin/bash -c "source /openvslam/ros/devel/setup.bash && rosrun openvslam run_slam -v /openvslam/build/orb_vocab/orb_vocab.dbow2 -c /openvslam/build/aist_living_lab_1/config.yaml --map-db map.msg" 12 | 13 | -------------------------------------------------------------------------------- /launch/roscloud_gqcnn_docker.launch: -------------------------------------------------------------------------------- 1 | 2 | # start rosbridge websocket server 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /launch/roscloud_gqcnn_launch.launch: -------------------------------------------------------------------------------- 1 | 2 | # start rosbridge websocket server 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /launch/roscloud_gqcnn_vpc.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /launch/roscloud_mpt.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /launch/roscloud_openvslam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /launch/roscloud_openvslam_vpc.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /launch/roscloud_talker.launch: -------------------------------------------------------------------------------- 1 | 2 | # start rosbridge websocket server 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /launch/roscloud_vpc.launch: -------------------------------------------------------------------------------- 1 | 2 | # start rosbridge websocket server 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /launch/talker.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | FogROS 4 | 0.0.0 5 | The FogROS package 6 | 7 | 8 | 9 | 10 | keplerc 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rosduct 54 | rospy 55 | std_msgs 56 | roscpp 57 | rosduct 58 | rospy 59 | std_msgs 60 | roscpp 61 | rosduct 62 | rospy 63 | std_msgs 64 | message_generation 65 | message_runtime 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /scripts/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BerkeleyAutomation/FogROS/6c8cd4572b96bb3a8139321e53d1bbb193b907a2/scripts/.DS_Store -------------------------------------------------------------------------------- /scripts/__pycache__/roscloud_base.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BerkeleyAutomation/FogROS/6c8cd4572b96bb3a8139321e53d1bbb193b907a2/scripts/__pycache__/roscloud_base.cpython-38.pyc -------------------------------------------------------------------------------- /scripts/roscloud_base.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import rospy 4 | import re 5 | import shutil 6 | import rospkg 7 | import boto3 8 | from botocore.exceptions import ClientError 9 | import os 10 | import paramiko 11 | from scp import SCPClient 12 | from io import StringIO 13 | from requests import get 14 | import time 15 | import random 16 | 17 | # ec2 console coloring 18 | CRED = '\033[91m' 19 | CEND = '\033[0m' 20 | 21 | aws_region = "us-west-1" 22 | 23 | def make_zip_file(dir_name, target_path): 24 | pwd, package_name = os.path.split(dir_name) 25 | return shutil.make_archive(base_dir = package_name, root_dir = pwd, format = "zip", base_name = target_path) 26 | 27 | def aws_create_security_group(ec2, ec2_security_group): 28 | response = ec2.describe_vpcs() 29 | vpc_id = response.get('Vpcs', [{}])[0].get('VpcId', '') 30 | 31 | try: 32 | response = ec2.create_security_group(GroupName=ec2_security_group, 33 | Description='DESCRIPTION', 34 | VpcId=vpc_id) 35 | security_group_id = response['GroupId'] 36 | print('Security Group Created %s in vpc %s.' % (security_group_id, vpc_id)) 37 | 38 | data = ec2.authorize_security_group_ingress( 39 | GroupId=security_group_id, 40 | IpPermissions=[ 41 | {'IpProtocol': '-1', 42 | 'FromPort': 0, 43 | 'ToPort': 65535, 44 | 'IpRanges': [{'CidrIp': '0.0.0.0/0'}] 45 | } 46 | ]) 47 | print('Ingress Successfully Set %s' % data) 48 | ec2_security_group_ids = [security_group_id] 49 | except ClientError as e: 50 | print(e) 51 | print("security group id is " + str(ec2_security_group_ids)) 52 | return ec2_security_group_ids 53 | 54 | def aws_generate_key_pair(ec2, ec2_key_name): 55 | ec2_keypair = ec2.create_key_pair(KeyName=ec2_key_name) 56 | ec2_priv_key = ec2_keypair['KeyMaterial'] 57 | with open("/home/ubuntu/" + ec2_key_name + ".pem", "w") as f: 58 | f.write(ec2_priv_key) 59 | print(ec2_priv_key) 60 | return ec2_priv_key 61 | 62 | 63 | def aws_create_instance(ec2_resource, ec2_key_name, ec2_security_group_ids, ec2_instance_type="t2.large", ec2_instance_disk_size = 8): 64 | # 65 | # start EC2 instance 66 | # note that we can start muliple instances at the same time 67 | # 68 | instances = ec2_resource.create_instances( 69 | ImageId= 'ami-0d255df33d23c5a9d', 70 | MinCount=1, 71 | MaxCount=1, 72 | InstanceType=ec2_instance_type, 73 | KeyName= ec2_key_name, 74 | SecurityGroupIds= ec2_security_group_ids, 75 | BlockDeviceMappings=[ 76 | { 77 | 'DeviceName': '/dev/sda1', 78 | 'Ebs': { 79 | 'VolumeSize': 30, 80 | 'VolumeType': 'standard' 81 | } 82 | } 83 | ] 84 | ) 85 | print("Have created the instance: ", instances) 86 | print("type: " + ec2_instance_type) 87 | instance = instances[0] 88 | # use the boto3 waiter 89 | print("wait for launching to finish") 90 | instance.wait_until_running() 91 | print("launch finished") 92 | # reload instance object 93 | instance.reload() 94 | #instance_dict = ec2.describe_instances().get('Reservations')[0] 95 | #print(instance_dict) 96 | return instance 97 | 98 | def prepare_launch_file(launch_file, magic_int, modify_launch=True): 99 | 100 | # 101 | # read in the launchfile 102 | # we also modify the launchfile IP address to this machine's public IP address 103 | # 104 | launch_file_dir , launch_file_name = os.path.split(launch_file) 105 | with open(launch_file) as f: 106 | launch_text = f.read() 107 | 108 | my_ip = get('https://checkip.amazonaws.com').text.strip() 109 | print("robot public address is ", my_ip) 110 | 111 | rosduct_launch_text = ''' 112 | 113 | 114 | rosbridge_ip: ''' + my_ip + ''' 115 | rosbridge_port: 9090 116 | 117 | 118 | 119 | ''' 120 | with open("/tmp/to_cloud" + magic_int + ".launch" , "w") as f: 121 | if ("rosduct" not in launch_text) and modify_launch : 122 | f.write(launch_text.replace("", rosduct_launch_text)) 123 | else: 124 | f.write(launch_text.replace("ROSBRIDGE_IP_HOLDER", my_ip)) 125 | 126 | # find all the ROS packages in the launchscript 127 | # package need to follow ros naming convention 128 | # i.e. flat namespace with lower case letters and underscore separators 129 | # then zip all the packages 130 | # currently we assume all the packages can be catkin_make 131 | rospack = rospkg.RosPack() 132 | packages = set(re.findall(r"pkg=\"[a-z_]*\"" ,launch_text)) 133 | packages.add("pkg=\"FogROS\"") 134 | print(packages) 135 | zip_paths = [] 136 | for package in packages: 137 | package = package.split("\"")[1] 138 | pkg_path = rospack.get_path(package) 139 | zip_path = make_zip_file(pkg_path, "/tmp/" + package) 140 | zip_paths.append(zip_path) 141 | return zip_paths 142 | 143 | 144 | def create_ec2_pipeline(rand_int, ec2_instance_type = "t2.large", image_id = "ami-05829bd3e68bcd415"): 145 | ec2_key_name = "foo" + rand_int 146 | ec2_security_group_name = 'SECURITY_GROUP_NAME' + rand_int 147 | ec2_resource = boto3.resource('ec2', aws_region) 148 | ec2 = boto3.client('ec2', aws_region) 149 | ec2_priv_key = aws_generate_key_pair(ec2, ec2_key_name) 150 | ec2_security_group_ids = aws_create_security_group(ec2, ec2_security_group_name) 151 | instance = aws_create_instance(ec2_resource, ec2_key_name, ec2_security_group_ids, ec2_instance_type) 152 | public_ip = instance.public_ip_address 153 | while not public_ip: 154 | instance.reload() 155 | public_ip = instance.public_ip_address 156 | print("public ip of ec2: " + public_ip) 157 | return public_ip, ec2_key_name 158 | 159 | 160 | 161 | 162 | def connect_and_launch(ec2_key_name, zip_paths, public_ip, launch_file_dir, env_script, magic_int, env_command= ""): 163 | private_key = paramiko.RSAKey.from_private_key_file("/home/ubuntu/" + ec2_key_name + ".pem") 164 | ssh_client = paramiko.SSHClient() 165 | ssh_client.set_missing_host_key_policy(paramiko.AutoAddPolicy()) 166 | ssh_client.connect(hostname = public_ip, username = "ubuntu", pkey = private_key, look_for_keys=False ) 167 | 168 | with SCPClient(ssh_client.get_transport()) as scp: 169 | # transfer all the zip files to the EC2 server's workspace 170 | for zip_file in zip_paths: 171 | scp.put(zip_file, '~/catkin_ws/src') 172 | 173 | # use SCP to upload the launch script 174 | # TODO: there might be a collision if we run multiple nodes, need to solve it 175 | scp.put("/tmp/to_cloud" + magic_int + ".launch", "~/catkin_ws/src/FogROS/launch/to_cloud.launch") 176 | 177 | scp.put(env_script, "~/setup.bash") 178 | 179 | 180 | # use SSH to unzip them to the catkin workspace 181 | stdin, stdout, stderr = ssh_client.exec_command("cd ~/catkin_ws/src && for i in *.zip; do unzip -o \"$i\" -d . ; done " , get_pty=True) 182 | 183 | for line in iter(stdout.readline, ""): 184 | continue 185 | #print(CRED + line + CEND, end="") 186 | 187 | # execute setup script 188 | stdin, stdout, stderr = ssh_client.exec_command("chmod +x ~/setup.bash && ~/setup.bash" , get_pty=True) 189 | 190 | for line in iter(stdout.readline, ""): 191 | print(CRED + line + CEND, end="") 192 | 193 | 194 | # catkin_make all the uploaded packages 195 | # roslaunch the script on EC2 196 | stdin, stdout, stderr = ssh_client.exec_command('cd ~/catkin_ws/ && source ./devel/setup.bash && catkin_make -DCMAKE_BUILD_TYPE=Release' , get_pty=True) 197 | 198 | for line in iter(stdout.readline, ""): 199 | print("EC2: " + str(time.time()) + " " + CRED + line + CEND, end="") 200 | 201 | 202 | stdin, stdout, stderr = ssh_client.exec_command(env_command + " echo $ROS_HOSTNAME && echo $ROS_MASTER_URI", get_pty=True) 203 | 204 | for line in iter(stdout.readline, ""): 205 | print("==============" + line, end="") 206 | print("=================") 207 | print(stderr) 208 | print(env_command + " roslaunch FogROS to_cloud.launch") 209 | 210 | stdin, stdout, stderr = ssh_client.exec_command("source ~/catkin_ws/devel/setup.bash && " + env_command + " roslaunch FogROS to_cloud.launch", get_pty=True) 211 | 212 | for line in iter(stdout.readline, ""): 213 | print("EC2: " + str(time.time()) + " " + CRED + line + CEND, end="") 214 | 215 | 216 | def push_vpc(launch_file, ec2_instance_type, env_script): 217 | rand_int = str(random.randint(10, 1000)) 218 | print("start launching " + str(time.time())) 219 | public_ip, ec2_key_name =create_ec2_pipeline(rand_int, ec2_instance_type) 220 | if(rospy.get_name() == "/leader"): 221 | with open("/tmp/leader_info", "w+") as f: 222 | f.write("{}".format(public_ip)) 223 | else: 224 | #TODO: depends on the task 225 | time.sleep(60) 226 | 227 | launch_file_dir , launch_file_name = os.path.split(launch_file) 228 | zip_paths = prepare_launch_file(launch_file, rand_int, False) 229 | time.sleep(240) 230 | 231 | leader_ip = "" 232 | with open("/tmp/leader_info") as f: 233 | leader_ip = f.read() 234 | with open(env_script) as f: 235 | env_script_txt = f.read() 236 | env_command = ''' 237 | export ROS_HOSTNAME={} 238 | export ROS_MASTER_URI=http://{}:11311 239 | '''.format(public_ip, leader_ip.strip()) 240 | env_script_text = env_command + env_script_txt 241 | if (public_ip != leader_ip): 242 | print("need to modify the ip env") 243 | env_script_text = env_script_text.replace("docker run", "docker run -e ROS_HOSTNAME={} -e ROS_MASTER_URI=http://{}:11311 ".format(public_ip, leader_ip.strip())) 244 | 245 | env_script = "/tmp/setup" + rand_int + ".bash" 246 | with open(env_script, "w+") as f: 247 | print(env_script_text) 248 | f.write(env_script_text) 249 | 250 | if public_ip == leader_ip: 251 | print("public == leader ===============") 252 | connect_and_launch(ec2_key_name, zip_paths, public_ip, launch_file_dir, env_script, rand_int) 253 | else: 254 | print("public != leader===========") 255 | connect_and_launch(ec2_key_name, zip_paths, public_ip, launch_file_dir, env_script, rand_int, "export ROS_HOSTNAME={} && export ROS_MASTER_URI=http://{}:11311 &&".format(public_ip, leader_ip.strip())) 256 | 257 | def push_docker(docker_image, ec2_instance_type): 258 | rand_int = str(random.randint(10, 1000)) 259 | print("start launching " + str(time.time())) 260 | docker_str =''' 261 | sudo apt install -y docker.io 262 | sudo docker pull ''' + docker_image + ''' 263 | sudo docker run -d --gpus all --network host --rm ''' + docker_image 264 | 265 | launch_file_str = ''' 266 | 267 | 268 | ''' 269 | launch_file = "/tmp/docker.launch" 270 | env_script = "/tmp/docker.bash" 271 | with open(launch_file, "w") as f: 272 | f.write(launch_file_str) 273 | public_ip, ec2_key_name =create_ec2_pipeline(rand_int, ec2_instance_type) 274 | launch_file_dir , launch_file_name = os.path.split(launch_file) 275 | zip_paths = prepare_launch_file(launch_file, rand_int) 276 | with open("/tmp/docker.bash", "w") as f: 277 | f.write(docker_str) 278 | 279 | time.sleep(60) 280 | connect_and_launch(ec2_key_name, zip_paths, public_ip, launch_file_dir, env_script, rand_int) 281 | 282 | 283 | def push_launch(launch_file, ec2_instance_type, env_script): 284 | rand_int = str(random.randint(10, 1000)) 285 | print("start launching " + str(time.time())) 286 | public_ip, ec2_key_name =create_ec2_pipeline(rand_int, ec2_instance_type) 287 | launch_file_dir , launch_file_name = os.path.split(launch_file) 288 | zip_paths = prepare_launch_file(launch_file, rand_int) 289 | 290 | time.sleep(60) 291 | connect_and_launch(ec2_key_name, zip_paths, public_ip, launch_file_dir, env_script, rand_int) 292 | -------------------------------------------------------------------------------- /scripts/roscloud_docker.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | import sys, os 3 | sys.path.append(os.path.dirname(os.path.realpath(__file__))) 4 | from roscloud_base import push_docker 5 | import rospy 6 | 7 | if __name__ == "__main__": 8 | rospy.init_node('roscloud') 9 | print(rospy.get_param_names()) 10 | docker_image = rospy.get_param('~docker_image') 11 | instance_type = rospy.get_param('~instance_type') 12 | push_docker(docker_image, instance_type) 13 | 14 | -------------------------------------------------------------------------------- /scripts/roscloud_launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | import sys, os 3 | sys.path.append(os.path.dirname(os.path.realpath(__file__))) 4 | from roscloud_base import push_launch 5 | import rospy 6 | 7 | if __name__ == "__main__": 8 | rospy.init_node('roscloud') 9 | print(rospy.get_name()) 10 | launch_file = rospy.get_param('~launch_file') 11 | instance_type = rospy.get_param("~instance_type") 12 | env_script = rospy.get_param("~env_script", "") 13 | if not env_script: 14 | with open("/tmp/empty_bash_env", "w") as f: 15 | f.write("echo Hello") 16 | env_script = "/tmp/empty_bash_env" 17 | push_launch(launch_file, instance_type, env_script) 18 | 19 | -------------------------------------------------------------------------------- /scripts/roscloud_vpc.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | import sys, os 3 | sys.path.append(os.path.dirname(os.path.realpath(__file__))) 4 | from roscloud_base import push_vpc 5 | import rospy 6 | 7 | if __name__ == "__main__": 8 | rospy.init_node('roscloud') 9 | print(rospy.get_name()) 10 | launch_file = rospy.get_param('~launch_file') 11 | instance_type = rospy.get_param("~instance_type") 12 | env_script = rospy.get_param("~env_script", "") 13 | if not env_script: 14 | with open("/tmp/empty_bash_env", "w") as f: 15 | f.write("echo Hello") 16 | env_script = "/tmp/empty_bash_env" 17 | push_vpc(launch_file, instance_type, env_script) 18 | 19 | -------------------------------------------------------------------------------- /scripts/rosduct_main.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import rospy 4 | from rosduct.rosduct_impl import ROSduct 5 | 6 | if __name__ == '__main__': 7 | rospy.init_node('rosduct') 8 | r = ROSduct() 9 | r.spin() 10 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | packages=['rosduct'], 8 | package_dir={'': 'src'} 9 | ) 10 | 11 | setup(**d) 12 | -------------------------------------------------------------------------------- /src/rosduct/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BerkeleyAutomation/FogROS/6c8cd4572b96bb3a8139321e53d1bbb193b907a2/src/rosduct/__init__.py -------------------------------------------------------------------------------- /src/rosduct/__pycache__/conversions.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BerkeleyAutomation/FogROS/6c8cd4572b96bb3a8139321e53d1bbb193b907a2/src/rosduct/__pycache__/conversions.cpython-38.pyc -------------------------------------------------------------------------------- /src/rosduct/__pycache__/rosbridge_client.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BerkeleyAutomation/FogROS/6c8cd4572b96bb3a8139321e53d1bbb193b907a2/src/rosduct/__pycache__/rosbridge_client.cpython-38.pyc -------------------------------------------------------------------------------- /src/rosduct/__pycache__/rosduct_impl.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BerkeleyAutomation/FogROS/6c8cd4572b96bb3a8139321e53d1bbb193b907a2/src/rosduct/__pycache__/rosduct_impl.cpython-38.pyc -------------------------------------------------------------------------------- /src/rosduct/conversions.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | from genpy.message import fill_message_args 4 | from pydoc import locate 5 | import json 6 | import yaml 7 | 8 | """ 9 | Conversions to-from ROS messages, JSON and dict 10 | representations of them. 11 | 12 | Author: Sammy Pfeiffer 13 | """ 14 | 15 | 16 | def get_ROS_msg_type(ros_msg): 17 | """ 18 | Returns the ROS message type as package_msgs/Message. 19 | :param AnyMsgInstance ros_msg: ROS message instance to 20 | get the type from. 21 | :returns str: ROS message type as package_msg/Message. 22 | """ 23 | return ros_msg._type 24 | 25 | 26 | def is_ros_message_installed(ros_message_type): 27 | """ 28 | Return true if the message is found installed. 29 | False otherwise. 30 | :param str ros_message_type: ROS message type as package_msgs/Message. 31 | :returns bool: True if is installed, False otherwise. 32 | """ 33 | try: 34 | package_name, message_name = ros_message_type.split('/') 35 | except ValueError: 36 | raise ValueError( 37 | 'ros_message_type should be in the shape of package_msgs/Message' + 38 | ' (it was ' + ros_message_type + ')') 39 | msg_class = locate('{}.msg.{}'.format(package_name, message_name)) 40 | if msg_class is None: 41 | return False 42 | else: 43 | return True 44 | 45 | 46 | def is_ros_service_installed(ros_message_type): 47 | """ 48 | Return true if the message is found installed. 49 | False otherwise. 50 | :param str ros_message_type: ROS message type as package_msgs/Message. 51 | :returns bool: True if is installed, False otherwise. 52 | """ 53 | try: 54 | package_name, message_name = ros_message_type.split('/') 55 | except ValueError: 56 | raise ValueError( 57 | 'ros_message_type should be in the shape of package_msgs/Message' + 58 | ' (it was ' + ros_message_type + ')') 59 | msg_class = locate('{}.srv.{}'.format(package_name, message_name)) 60 | if msg_class is None: 61 | return False 62 | else: 63 | return True 64 | 65 | 66 | def get_ROS_class(ros_message_type, srv=False): 67 | """ 68 | Returns the ROS message class from ros_message_type. 69 | :return AnyMsgClass: Class of the ROS message. 70 | """ 71 | try: 72 | package_name, message_name = ros_message_type.split('/') 73 | except ValueError: 74 | raise ValueError( 75 | 'ros_message_type should be in the shape of package_msgs/Message' + 76 | ' (it was ' + ros_message_type + ')') 77 | if not srv: 78 | msg_class = locate('{}.msg.{}'.format(package_name, message_name)) 79 | else: 80 | msg_class = locate('{}.srv.{}'.format(package_name, message_name)) 81 | if msg_class is None: 82 | if srv: 83 | msg_or_srv = '.srv' 84 | else: 85 | msg_or_srv = '.msg' 86 | raise ValueError( 87 | 'ros_message_type could not be imported. (' + 88 | ros_message_type + ', as "from ' + package_name + 89 | msg_or_srv + ' import ' + message_name + '" failed.') 90 | return msg_class 91 | 92 | 93 | def from_ROS_to_dict(ros_msg): 94 | """ 95 | Converts from a ROS message instance to a dict representation 96 | of it. 97 | :param AnyMsgInstance ros_msg: ROS message instance to transform to 98 | a dictionary. 99 | :returns dict: dictionary representing the ROS message. 100 | """ 101 | # Note that this may be very slow for big data structures like 102 | # an sensor_msgs/Image 103 | return yaml.safe_load(ros_msg.__str__()) 104 | 105 | 106 | def from_dict_to_ROS(dict_msg, ros_message_type, srv=False): 107 | """ 108 | Converts from a dict representation of a ROS message to a 109 | ROS message instance. 110 | """ 111 | msg_class = get_ROS_class(ros_message_type, srv=srv) 112 | #print(msg_class) 113 | msg_instance = msg_class() 114 | #print(msg_instance.__slots__) 115 | #print(msg_instance._slot_types) 116 | #print(dict_msg.keys()) 117 | if "data" in dict_msg.keys(): 118 | import base64 119 | dict_msg["data"] = base64.b64decode(dict_msg["data"]) 120 | print(dict_msg["data"][:10]) 121 | #dict_msg["data"] = (dict_msg["data"]).encode() 122 | 123 | if "min_solution_cost" in dict_msg.keys(): 124 | print("solution cost", str(dict_msg["min_solution_cost"])) 125 | 126 | #dict_msg["min_solution_cost"] = 0.0 127 | #for message_slot in msg_instance.__slots__: 128 | # print(message_slot, dict_msg[message_slot]) 129 | # Workaround 130 | if len(dict_msg) == 1: 131 | dict_msg = [dict_msg] 132 | fill_message_args(msg_instance, dict_msg) 133 | #print("instance: " + str(msg_instance) + str(len(msg_instance.data))) 134 | print(str(msg_instance)[:200]) 135 | return msg_instance 136 | 137 | 138 | def from_JSON_to_ROS(json_msg, ros_message_type, srv=False): 139 | """ 140 | Converts from a dict representation of a ROS message to a 141 | ROS message instance. 142 | """ 143 | return from_dict_to_ROS(from_JSON_to_dict(json_msg), 144 | ros_message_type, srv=srv) 145 | 146 | 147 | def from_JSON_to_dict(json_msg): 148 | """ 149 | Converts from a json representation of a ROS message 150 | to a dict representation. 151 | """ 152 | return json.loads(json_msg) 153 | 154 | 155 | def from_dict_to_JSON(dict_msg): 156 | """ 157 | Converts from a dict representation fo a ROS message 158 | to a JSON representation of it. 159 | """ 160 | return json.dumps(dict_msg) 161 | 162 | 163 | def from_ROS_to_JSON(ros_msg): 164 | """ 165 | Converts from a ROS message instance to a JSON representation 166 | of it. 167 | """ 168 | dict_msg = from_ROS_to_dict(ros_msg) 169 | return from_dict_to_JSON(dict_msg) 170 | 171 | 172 | if __name__ == '__main__': 173 | from std_msgs.msg import Header 174 | h_ROS = Header() 175 | print("ROS message is:\n" + str(h_ROS)) 176 | 177 | print("--------------------") 178 | print("From ROS message...") 179 | h_dict = from_ROS_to_dict(h_ROS) 180 | print("... to dict:\n" + str(h_dict)) 181 | h_JSON = from_ROS_to_JSON(h_ROS) 182 | print("... to JSON:\n" + str(h_JSON)) 183 | 184 | print("--------------------") 185 | print("From dict to...") 186 | h_ROS_from_dict = from_dict_to_ROS(h_dict, get_ROS_msg_type(h_ROS)) 187 | print("... to ROS message:\n" + str(h_ROS_from_dict)) 188 | h_JSON_from_dict = from_dict_to_JSON(h_dict) 189 | print("... to JSON:\n" + str(h_JSON_from_dict)) 190 | 191 | print("--------------------") 192 | print("From JSON to...") 193 | h_ROS_from_JSON = from_JSON_to_ROS(h_JSON, get_ROS_msg_type(h_ROS)) 194 | print("... to ROS message:\n" + str(h_ROS_from_JSON)) 195 | h_dict_from_JSON = from_JSON_to_dict(h_JSON) 196 | print("... to dict:\n" + str(h_dict_from_JSON)) 197 | 198 | print(("--------------------")) 199 | if h_ROS == h_ROS_from_dict: 200 | print("from_dict_to_ROS works.") 201 | else: 202 | print("from_dict_to_ROS error.") 203 | 204 | if h_ROS == h_ROS_from_JSON: 205 | print("from_JSON_to_ROS works.") 206 | else: 207 | print("from_JSON_to_ROS error.") 208 | 209 | if h_dict == h_dict_from_JSON: 210 | print("from_JSON_to_dict works.") 211 | else: 212 | print("from_JSON_to_dict error.") 213 | 214 | if h_JSON == h_JSON_from_dict: 215 | print("from_dict_to_JSON works.") 216 | else: 217 | print("from_dict_to_JSON error.") 218 | 219 | if h_dict == h_dict_from_JSON: 220 | print("from_ROS_to_dict works.") 221 | else: 222 | print("from_ROS_to_dict error.") 223 | 224 | if h_JSON == h_JSON_from_dict: 225 | print("from_ROS_to_JSON works.") 226 | else: 227 | print("from_ROS_to_JSON error.") 228 | -------------------------------------------------------------------------------- /src/rosduct/rosbridge_client.py: -------------------------------------------------------------------------------- 1 | """This module provides a python client for rosbridge to publish, subscribe topics, 2 | call services, create service server and use action client. 3 | """ 4 | 5 | import threading 6 | import time 7 | import json 8 | import uuid 9 | from ast import literal_eval 10 | from ws4py.client.threadedclient import WebSocketClient 11 | # sudo pip install PyDispatcher 12 | from pydispatch import dispatcher 13 | 14 | 15 | class ROSBridgeClient(WebSocketClient): 16 | """ROSBridgeClient extends WebSocketClient and manages connection to the 17 | server and all interactions with ROS. 18 | 19 | It keeps a record of all publishers, subscriber, service request callbacks, 20 | service servers and action clients. 21 | """ 22 | 23 | def __init__(self, ip, port=9090): 24 | """Constructor for ROSBridgeClient. 25 | 26 | Args: 27 | ip (str): The robot IP address. 28 | port (int, optional): The WebSocket port number for rosbridge. 29 | Defaults to 9090. 30 | """ 31 | WebSocketClient.__init__(self, 'ws://{}:{}'.format(ip, port)) 32 | self._connected = False 33 | self._id_counter = 0 34 | self._publishers = {} 35 | self._subscribers = {} 36 | self._service_clients = {} 37 | self._service_servers = {} 38 | self._action_clients = {} 39 | self.connect() 40 | threading.Thread(target=self.run_forever).start() 41 | while not self._connected: 42 | time.sleep(0.1) 43 | 44 | @property 45 | def id_counter(self): 46 | """Generate an auto-incremental ID starts from 1. 47 | 48 | Returns: 49 | A auto-incremented ID. 50 | """ 51 | self._id_counter += 1 52 | return self._id_counter 53 | 54 | def get_topics(self): 55 | """Get topic list, like using `rostopic list`. 56 | 57 | Returns: 58 | A list of topic names. 59 | """ 60 | service_client = self.service_client('/rosapi/topics', 'rosapi/Topics') 61 | result = { 62 | 'responded': False, 63 | 'topics': [] 64 | } 65 | 66 | def cb(success, values): 67 | result['responded'] = True 68 | if success: 69 | result['topics'] = values.get('topics') 70 | 71 | service_client.request({}, cb) 72 | while not result.get('responded'): 73 | time.sleep(0.1) 74 | return result.get('topics') 75 | 76 | def get_topic_type(self, topic_name): 77 | """Get the type of a topic with given name, like using `rostopic type`. 78 | 79 | Args: 80 | topic_name (str): The name of the topic. 81 | 82 | Returns: 83 | The type of the given topic 84 | """ 85 | service_client = self.service_client( 86 | '/rosapi/topic_type', 'rosapi/TopicType') 87 | result = { 88 | 'responded': False, 89 | 'type': None 90 | } 91 | 92 | def cb(success, values): 93 | result['responded'] = True 94 | if success: 95 | result['type'] = values.get('type') 96 | 97 | service_client.request({'topic': topic_name}, cb) 98 | while not result.get('responded'): 99 | time.sleep(0.1) 100 | return result.get('type') 101 | 102 | def get_services(self): 103 | """Get topic list, like using `rosservice list`. 104 | 105 | Returns: 106 | A list of service names. 107 | """ 108 | service_client = self.service_client( 109 | '/rosapi/services', 'rosapi/Services') 110 | result = { 111 | 'responded': False, 112 | 'services': [] 113 | } 114 | 115 | def cb(success, values): 116 | result['responded'] = True 117 | if success: 118 | result['services'] = values.get('services') 119 | 120 | service_client.request({}, cb) 121 | while not result.get('responded'): 122 | time.sleep(0.1) 123 | return result.get('services') 124 | 125 | def get_service_type(self, service_name): 126 | """Get the type of a service with given name, like using `rosservice type`. 127 | 128 | Args: 129 | service_name (str): The name of the service. 130 | 131 | Returns: 132 | The type of the given service 133 | """ 134 | service_client = self.service_client( 135 | '/rosapi/service_type', 'rosapi/ServiceType') 136 | result = { 137 | 'responded': False, 138 | 'type': None 139 | } 140 | 141 | def cb(success, values): 142 | result['responded'] = True 143 | if success: 144 | result['type'] = values.get('type') 145 | 146 | service_client.request({'service': service_name}, cb) 147 | while not result.get('responded'): 148 | time.sleep(0.1) 149 | return result.get('type') 150 | 151 | def get_params_names(self): 152 | """Get params list, like using `rosparam list`. 153 | 154 | Returns: 155 | A list of parameter names. 156 | """ 157 | service_client = self.service_client( 158 | '/rosapi/get_param_names', 'rosapi/GetParamNames') 159 | result = { 160 | 'responded': False, 161 | 'params': [] 162 | } 163 | 164 | def cb(success, values): 165 | result['responded'] = True 166 | if success: 167 | result['params'] = values.get('names') 168 | 169 | service_client.request({}, cb) 170 | while not result.get('responded'): 171 | time.sleep(0.1) 172 | return result.get('params') 173 | 174 | def get_param(self, param_name, default_value=None): 175 | """Get the value of a parameter with the given name, like using `rosparam get`. 176 | 177 | Args: 178 | param_name (str): The name of the parameter. 179 | 180 | Returns: 181 | The value of the parameter if exist, None otherwise. 182 | """ 183 | service_client = self.service_client( 184 | '/rosapi/get_param', 'rosapi/GetParam') 185 | result = { 186 | 'responded': False, 187 | 'value': None 188 | } 189 | 190 | def cb(success, values): 191 | result['responded'] = True 192 | if success: 193 | result['value'] = values.get('value') 194 | result['success'] = True 195 | 196 | service_client.request({'name': param_name}, cb) 197 | while not result.get('responded'): 198 | time.sleep(0.1) 199 | # skip unicode 200 | param_value = str(result.get('value')) 201 | if param_value == 'null': 202 | return default_value 203 | # JSON returns true and false in lower case starting letter... 204 | if param_value == 'false' or param_value == 'true': 205 | param_value = param_value[0].upper() + param_value[1:] 206 | return literal_eval(param_value) 207 | 208 | def set_param(self, param_name, value): 209 | """Get the value of a parameter with the given name, like using `rosparam get`. 210 | 211 | Args: 212 | param_name (str): The name of the parameter. 213 | value: The new value to be set for parameter with the given name. 214 | 215 | Returns: 216 | Whether the set operation is successful or not. 217 | """ 218 | service_client = self.service_client( 219 | '/rosapi/set_param', 'rosapi/SetParam') 220 | result = { 221 | 'responded': False, 222 | 'succeed': False 223 | } 224 | 225 | def cb(success, values): 226 | result['succeed'] = success 227 | result['responded'] = True 228 | 229 | service_client.request( 230 | {'name': param_name, 'value': json.dumps(value)}, cb) 231 | while not result.get('responded'): 232 | time.sleep(0.1) 233 | return result.get('succeed') 234 | 235 | def publisher(self, topic_name, message_type, latch=False, queue_size=1): 236 | """Create a _Publisher object if the given topic hasn't been advertised, 237 | otherwise return the existing 238 | publisher that is currently advertising the topic. 239 | 240 | Args: 241 | topic_name (str): The ROS topic name. 242 | message_type (str): The ROS message type as `std_msgs/String`. 243 | latch (bool, optional): Wether the topic is latched when publishing. 244 | Defaults to False. 245 | queue_size (int): The queue created at bridge side for republishing. 246 | Defaults to 1. 247 | 248 | Returns: 249 | A _Publisher object. 250 | """ 251 | if topic_name in self._publishers: 252 | publisher = self._publishers.get(topic_name) 253 | publisher.usage += 1 254 | else: 255 | print('Advertising topic {} for publishing'.format(topic_name)) 256 | publisher = _Publisher( 257 | self, topic_name, message_type, latch, queue_size) 258 | self._publishers[topic_name] = publisher 259 | return publisher 260 | 261 | def unregister_publisher(self, topic_name): 262 | """Stop advertising on the given topic. 263 | 264 | Args: 265 | topic_name (str): The ROS topic name. 266 | """ 267 | if topic_name in self._publishers: 268 | print('Stop advertising topic {} for publishing'.format(topic_name)) 269 | del self._publishers[topic_name] 270 | 271 | def subscriber(self, topic_name, message_type, cb): 272 | """Create a _Subscriber object on a given topic with a callback function. 273 | 274 | If the topic hasn't been subscribed yet, subscribe the topic. 275 | Otherwise, it adds the subscriber 276 | with callback function into the topic subscription list. 277 | 278 | Args: 279 | topic_name (str): The ROS topic name. 280 | message_type (str): The ROS message type, such as std_msgs/String. 281 | cb (function): A function will be called when a message is 282 | received on that topic. 283 | 284 | Returns: 285 | A _Subscriber object. 286 | """ 287 | subscriber = _Subscriber(self, topic_name, cb) 288 | if topic_name in self._subscribers: 289 | self._subscribers.get(topic_name).get( 290 | 'subscribers').append(subscriber) 291 | else: 292 | subscribe_id = 'subscribe:{}:{}'.format( 293 | topic_name, self._id_counter) 294 | print('Sending request to subscribe topic {}'.format(topic_name)) 295 | self.send(json.dumps({ 296 | 'op': 'subscribe', 297 | 'id': subscribe_id, 298 | 'topic': topic_name, 299 | 'type': message_type 300 | })) 301 | self._subscribers[topic_name] = {} 302 | self._subscribers[topic_name]['subscribe_id'] = subscribe_id 303 | self._subscribers[topic_name]['subscribers'] = [subscriber] 304 | return subscriber 305 | 306 | def unsubscribe(self, subscriber): 307 | """Remove a callback subscriber from its topic subscription list. 308 | 309 | If there is no callback subscribers in the subscription list. 310 | It will unsubscribe the topic. 311 | 312 | Args: 313 | subscriber (_Subscriber): A subscriber with callback function 314 | that listen to the topic. 315 | """ 316 | topic_name = subscriber.topic_name 317 | if topic_name not in self._subscribers: 318 | return 319 | subscribe_id = self._subscribers.get(topic_name).get('subscribe_id') 320 | subscribers = self._subscribers.get(topic_name).get('subscribers') 321 | if subscriber in subscribers: 322 | subscribers.remove(subscriber) 323 | if len(subscribers) == 0: 324 | print('Sending request to unsubscribe topic {}'.format(topic_name)) 325 | del subscribers[:] 326 | self.send(json.dumps({ 327 | 'op': 'unsubscribe', 328 | 'id': subscribe_id, 329 | 'topic': topic_name 330 | })) 331 | del self._subscribers[topic_name] 332 | 333 | def service_client(self, service_name, service_type): 334 | """Create a ROS service client. 335 | 336 | Args: 337 | service_name (str): The ROS service name. 338 | service_type (str): The ROS service type. 339 | 340 | Returns: 341 | A _ServiceClient object. 342 | """ 343 | return _ServiceClient(self, service_name, service_type) 344 | 345 | def register_service_callback(self, service_id, cb): 346 | """Register a service callback with a service request ID. 347 | 348 | Args: 349 | service_id (str): The service request ID. 350 | cb (function): A function will be called when the service 351 | server responses. 352 | """ 353 | self._service_clients[service_id] = cb 354 | 355 | def service_server(self, service_name, service_type, handler): 356 | """Create a ROS service server. 357 | 358 | Args: 359 | service_name (str): The ROS service name. 360 | service_type (str): The ROS service type. 361 | handler (function): A callback function takes the service 362 | request and gives service response. 363 | 364 | Returns: 365 | A _ServiceServer object 366 | """ 367 | if service_name in self._service_servers: 368 | self._service_servers[service_name].unregister() 369 | service_server = _ServiceServer( 370 | self, service_name, service_type, handler) 371 | self._service_servers[service_name] = service_server 372 | return service_server 373 | 374 | def register_service_server(self, service_name, handler): 375 | """Register a service server handler with associated service name. 376 | 377 | Args: 378 | service_name (str): The ROS service name. 379 | handler (function): A callback function takes the service request 380 | and gives service response. 381 | """ 382 | self._service_servers[service_name] = handler 383 | 384 | def unregister_service_server(self, service_name): 385 | """Remove a service server with given name. 386 | 387 | Args: 388 | service_name (str): The ROS service name. 389 | """ 390 | if service_name in self._service_servers: 391 | print('Stop advertising service server {} for publishing'.format( 392 | service_name)) 393 | del self._service_servers[service_name] 394 | 395 | def action_client(self, server_name, action_name): 396 | """Create a ROS action client if there was no client created for 397 | the action server. Otherwise return that action client. 398 | 399 | Args: 400 | server_name (str): The ROS action server name. 401 | action_name (str): The ROS action name. 402 | 403 | Returns: 404 | A _ActionClient object. 405 | """ 406 | if server_name + ':' + action_name in self._action_clients: 407 | action_client = self._action_clients.get( 408 | server_name + ':' + action_name).get('action_client') 409 | action_client.usage += 1 410 | else: 411 | action_client = _ActionClient(self, server_name, action_name) 412 | return action_client 413 | 414 | def unregister_action_client(self, server_name, action_name): 415 | """Unregister the action client with server and action name. Remove 416 | it from the internal action client dict. 417 | 418 | Args: 419 | server_name (str): The ROS action server name. 420 | action_name (str): The ROS action name. 421 | """ 422 | if server_name + ':' + action_name in self._action_clients: 423 | del self._action_clients[server_name + ':' + action_name] 424 | 425 | def opened(self): 426 | """Called when the connection to ROS established.""" 427 | self._connected = True 428 | print('Connected with rosbridge') 429 | 430 | def closed(self, code, reason=None): 431 | """Called when the connection to ROS disconnected 432 | 433 | Args: 434 | code (int): A status code. 435 | reason (str, opitonal): A human readable message. Defaults to None. 436 | """ 437 | print('Disconnected with rosbridge') 438 | 439 | def received_message(self, message): 440 | """Called when message received from ROS server. 441 | 442 | Only handle the message with `topic` or `service` keywords and trigger 443 | corresponding callback functions. 444 | 445 | Args: 446 | message(ws4py.messaging.Message): A message that sent from 447 | ROS server. 448 | """ 449 | #print(message) 450 | data = json.loads(message.data) 451 | if 'topic' in data: 452 | # Note that the callback argument MUST be named message (damn.) 453 | dispatcher.send(signal=data.get('topic'), message=data.get('msg')) 454 | if 'service' in data: 455 | if data.get('op') == 'service_response': 456 | service_id = data.get('id') 457 | success = data.get('result') 458 | values = data.get('values') 459 | if service_id in self._service_clients: 460 | self._service_clients[service_id](success, values) 461 | del self._service_clients[service_id] 462 | elif data.get('op') == 'call_service': 463 | service_name = data.get('service') 464 | service_id = data.get('id') 465 | args = data.get('args') 466 | if service_name in self._service_servers: 467 | result, values = self._service_servers[service_name].run_handler( 468 | args) 469 | self.send(json.dumps({ 470 | 'op': 'service_response', 471 | 'service': service_name, 472 | 'id': service_id, 473 | 'result': result, 474 | 'values': values 475 | })) 476 | 477 | def unhandled_error(self, error): 478 | """Called when a socket or OS error is raised. 479 | 480 | Args: 481 | error (str): A human readable error message. 482 | """ 483 | print(error) 484 | 485 | def __del__(self): 486 | for publisher in self._publishers: 487 | self._publishers[publisher].unregister() 488 | for subscriber in self._subscribers: 489 | self._subscribers[subscriber].unregister() 490 | for service_server in self._service_servers: 491 | self._service_servers[service_server].unregister() 492 | for action_client in self._action_clients: 493 | self._action_clients[action_client].unregister() 494 | self.close() 495 | 496 | 497 | class _Publisher(object): 498 | def __init__(self, rosbridge, topic_name, message_type, 499 | latch=False, queue_size=1): 500 | """Constructor for _Publisher. 501 | 502 | Args: 503 | rosbridge (ROSBridgeClient): The ROSBridgeClient object. 504 | topic_name (str): The ROS topic name. 505 | message_type (str): The ROS message type, such as std_msgs/String. 506 | latch (bool, optional): Whether the topic is latched when publishing. 507 | Defaults to False. 508 | queue_size (int): The queue created at bridge side for re-publishing. 509 | Defaults to 1. 510 | """ 511 | self._advertise_id = 'advertise:{}:{}'.format( 512 | topic_name, rosbridge.id_counter) 513 | self._rosbridge = rosbridge 514 | self._topic_name = topic_name 515 | self._usage = 1 516 | 517 | rosbridge.send(json.dumps({ 518 | 'op': 'advertise', 519 | 'id': self._advertise_id, 520 | 'topic': topic_name, 521 | 'type': message_type, 522 | 'latch': latch, 523 | 'queue_size': queue_size 524 | })) 525 | 526 | @property 527 | def usage(self): 528 | return self._usage 529 | 530 | @usage.setter 531 | def usage(self, value): 532 | self._usage = value 533 | 534 | def publish(self, message): 535 | """Publish a ROS message 536 | 537 | Args: 538 | message (dict): A message to send. 539 | """ 540 | self._rosbridge.send(json.dumps({ 541 | 'op': 'publish', 542 | 'id': 'publish:{}:{}'.format(self._topic_name, 543 | self._rosbridge.id_counter), 544 | 'topic': self._topic_name, 545 | 'msg': message 546 | })) 547 | 548 | def unregister(self): 549 | """Reduce the usage of the publisher. If the usage is 0, 550 | unadvertise this topic.""" 551 | self._usage -= 1 552 | if self._usage <= 0: 553 | self._rosbridge.unregister_publisher(self._topic_name) 554 | self._rosbridge.send(json.dumps({ 555 | 'op': 'unadvertise', 556 | 'id': self._advertise_id, 557 | 'topic': self._topic_name 558 | })) 559 | 560 | 561 | class _Subscriber(object): 562 | def __init__(self, rosbridge, topic_name, cb=None): 563 | """Constructor for _Subscriber. 564 | 565 | Args: 566 | rosbridge (ROSBridgeClient): The ROSBridgeClient object. 567 | topic_name (str): The ROS topic name. 568 | cb (function): A function will be called when a message is 569 | received on that topic. 570 | """ 571 | self._rosbridge = rosbridge 572 | self._topic_name = topic_name 573 | self._cb = cb 574 | if callable(self._cb): 575 | dispatcher.connect(self._cb, signal=topic_name) 576 | 577 | @property 578 | def topic_name(self): 579 | return self._topic_name 580 | 581 | def unregister(self): 582 | """Remove the current callback function from listening to the topic, 583 | and from the rosbridge client subscription list 584 | """ 585 | if callable(self._cb): 586 | dispatcher.disconnect(self._cb, signal=self._topic_name) 587 | self._rosbridge.unsubscribe(self) 588 | 589 | 590 | class _ServiceClient(object): 591 | def __init__(self, rosbridge, service_name, service_type): 592 | """Constructor for _ServiceClient. 593 | 594 | Args: 595 | rosbridge (ROSBridgeClient): The ROSBridgeClient object. 596 | service_name (str): The ROS service name. 597 | service_type (str): The ROS service type. 598 | """ 599 | self._rosbridge = rosbridge 600 | self._service_name = service_name 601 | self._service_type = service_type 602 | 603 | def request(self, request, cb): 604 | """Send a request to the ROS service server. The callback function 605 | will be called when service responses. 606 | 607 | Args: 608 | request (dict): A request message to send, 609 | cb (function): A function will be called when the service server 610 | responses. 611 | """ 612 | service_id = 'call_service:{}:{}'.format( 613 | self._service_name, self._rosbridge.id_counter) 614 | if callable(cb): 615 | self._rosbridge.register_service_callback(service_id, cb) 616 | self._rosbridge.send(json.dumps({ 617 | 'op': 'call_service', 618 | 'id': service_id, 619 | 'service': self._service_name, 620 | 'args': request 621 | })) 622 | 623 | 624 | class _ServiceServer(object): 625 | def __init__(self, rosbridge, service_name, service_type, handler): 626 | """Constructor for _ServiceServer. 627 | 628 | Args: 629 | rosbridge (ROSBridgeClient): The ROSBridgeClient object. 630 | service_name (str): The ROS service name. 631 | service_type (str): The ROS service type. 632 | handler (function): A callback function takes the service request 633 | and gives service response. 634 | """ 635 | self._rosbridge = rosbridge 636 | self._service_name = service_name 637 | self._service_type = service_type 638 | self._handler = handler 639 | 640 | self._rosbridge.send(json.dumps({ 641 | 'op': 'advertise_service', 642 | 'type': self._service_type, 643 | 'service': self._service_name 644 | })) 645 | 646 | def unregister(self): 647 | """Unadvertise the service""" 648 | self._rosbridge.unregister_service_server(self._service_name) 649 | self._rosbridge.send(json.dumps({ 650 | 'op': 'unadvertise_service', 651 | 'service': self._service_name 652 | })) 653 | 654 | def run_handler(self, args): 655 | """Execute handler function to give service response 656 | 657 | Args: 658 | args (dict): The request sent by the service client. 659 | """ 660 | if callable(self._handler): 661 | return self._handler(args) 662 | return False, None 663 | 664 | 665 | class _ActionClient(object): 666 | def __init__(self, rosbridge, server_name, action_name): 667 | """Constructor for _ActionClient 668 | 669 | Args: 670 | rosbridge (ROSBridgeClient): The ROSBridgeClient object. 671 | server_name (str): The ROS action server name. 672 | action_name (str): The ROS action name. 673 | """ 674 | self._rosbridge = rosbridge 675 | self._server_name = server_name 676 | self._action_name = action_name 677 | self._goals = {} 678 | self._usage = 1 679 | 680 | self._feedback_sub = rosbridge.subscriber( 681 | server_name + '/feedback', action_name + 'Feedback', 682 | self.on_feedback) 683 | self._result_sub = rosbridge.subscriber( 684 | server_name + '/result', action_name + 'Result', self.on_result) 685 | 686 | self._goal_pub = rosbridge.publisher( 687 | server_name + '/goal', action_name + 'Goal') 688 | self._cancel_pub = rosbridge.publisher( 689 | server_name + '/cancel', 'actionlib_msgs/GoalID') 690 | 691 | @property 692 | def usage(self): 693 | return self._usage 694 | 695 | @usage.setter 696 | def usage(self, value): 697 | self._usage = value 698 | 699 | def on_feedback(self, message): 700 | """Callback when a feedback message received. 701 | 702 | Args: 703 | message (dict): A feedback message received from ROS action server. 704 | """ 705 | goal = self._goals.get(message.get('status').get('goal_id').get('id')) 706 | if goal: 707 | goal.feedback_received(message.get( 708 | 'feedback'), message.get('status')) 709 | 710 | def on_result(self, message): 711 | """Callback when a result message received. 712 | 713 | Args: 714 | message (dict): A result message received from ROS action server. 715 | """ 716 | goal = self._goals.get(message.get('status').get('goal_id').get('id')) 717 | if goal: 718 | goal.result_received(message.get('result'), message.get('status')) 719 | 720 | def send_goal(self, goal_message, on_result, on_feedback): 721 | """Send a goal to the ROS action server. 722 | 723 | Args: 724 | goal_message (dict): A message to send to ROS action server. 725 | on_result (function): A callback function to be called when a 726 | feedback message received. 727 | on_feedback (function): A callback function to be called when a 728 | result message received. 729 | """ 730 | goal = _Goal(goal_message, on_result, on_feedback) 731 | self._goals[goal.id] = goal 732 | self._goal_pub.publish(goal.message) 733 | return goal.id 734 | 735 | def cancel_goal(self, goal_id): 736 | """Cancel a goal with a given goal ID 737 | 738 | Args: 739 | goal_id (str): The ID of the goal to be cancelled. 740 | """ 741 | self._cancel_pub.publish({'id': goal_id}) 742 | 743 | def unregister(self): 744 | """Reduce the usage of the action client. If the usage is 0, 745 | unregister its publishers and subscribers.""" 746 | self._usage -= 1 747 | if self._usage == 0: 748 | self._feedback_sub.unregister() 749 | self._result_sub.unregister() 750 | self._goal_pub.unregister() 751 | self._cancel_pub.unregister() 752 | self._rosbridge.unregister_action_client( 753 | self._server_name, self._action_name) 754 | 755 | 756 | class _Goal(object): 757 | def __init__(self, message, on_result=None, on_feedback=None): 758 | """Constructor for _Goal 759 | 760 | Args: 761 | message (dict): The goal message to send to ROS action server. 762 | on_result (function): A callback function to be called when a 763 | feedback message received. 764 | on_feedback (function): A callback function to be called when a 765 | result message received. 766 | """ 767 | self._id = 'goal_' + str(uuid.uuid4()) 768 | self._message = message 769 | self._is_finished = False 770 | self._on_result = on_result 771 | self._on_feedback = on_feedback 772 | 773 | @property 774 | def id(self): 775 | return self._id 776 | 777 | @property 778 | def message(self): 779 | """Wrap message in JSON format that complies ROSBridge protocol. 780 | 781 | Returns: 782 | A JSON that contains the goal ID and message. 783 | """ 784 | return { 785 | 'goal_id': { 786 | 'stamp': { 787 | 'secs': 0, 788 | 'nsecs': 0 789 | }, 790 | 'id': self._id 791 | }, 792 | 'goal': self._message 793 | } 794 | 795 | @property 796 | def is_finished(self): 797 | return self._is_finished 798 | 799 | def result_received(self, result, status): 800 | """Called when a result message is received from the Action Server (AS). 801 | 802 | Args: 803 | result (dict): The result message. 804 | status (int): The status code. Such as: 805 | ACTIVE = 1: The goal is currently being processed by the AS; 806 | PREEMPTED = 2: The goal received a cancel request after it 807 | started executing; 808 | SUCCEEDED = 3: The goal was achieved successfully by the AS; 809 | ABORTED = 4: The goal was aborted during execution by the AS 810 | due to some failure. 811 | For more details, refer to 812 | http://docs.ros.org/indigo/api/actionlib_msgs/html/msg/GoalStatus.html. 813 | """ 814 | self._is_finished = True 815 | if callable(self._on_result): 816 | self._on_result(result, status) 817 | 818 | def feedback_received(self, feedback, status): 819 | """Called when a result message is received. 820 | 821 | Args: 822 | feedback (dict): The feedback message. 823 | status (int): The status code. Such as: 824 | ACTIVE = 1: The goal is currently being processed by the AS; 825 | PREEMPTED = 2: The goal received a cancel request after it 826 | started executing; 827 | SUCCEEDED = 3: The goal was achieved successfully by the AS; 828 | ABORTED = 4: The goal was aborted during execution by the AS 829 | due to some failure. 830 | For more details, refer to 831 | http://docs.ros.org/indigo/api/actionlib_msgs/html/msg/GoalStatus.html. 832 | """ 833 | if callable(self._on_feedback): 834 | self._on_feedback(feedback, status) 835 | -------------------------------------------------------------------------------- /src/rosduct/rosduct_impl.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rospy 4 | from .conversions import from_dict_to_JSON 5 | from .conversions import from_JSON_to_dict 6 | from .conversions import from_dict_to_ROS 7 | from .conversions import from_ROS_to_dict 8 | from .conversions import from_JSON_to_ROS 9 | from .conversions import from_ROS_to_JSON 10 | from .conversions import get_ROS_msg_type 11 | from .conversions import get_ROS_class 12 | from .conversions import is_ros_message_installed, is_ros_service_installed 13 | from pydoc import locate 14 | import socket 15 | 16 | from .rosbridge_client import ROSBridgeClient 17 | 18 | """ 19 | Server to expose locally and externally 20 | topics, services and parameters from a remote 21 | roscore to a local roscore. 22 | 23 | Author: Sammy Pfeiffer 24 | """ 25 | 26 | yaml_config = ''' 27 | # ROSbridge websocket server info 28 | rosbridge_ip: 192.168.1.31 29 | rosbridge_port: 9090 30 | # Topics being published in the robot to expose locally 31 | remote_topics: [ 32 | ['/joint_states', 'sensor_msgs/JointState'], 33 | ['/tf', 'tf2_msgs/TFMessage'], 34 | ['/scan', 'sensor_msgs/LaserScan'] 35 | ] 36 | # Topics being published in the local roscore to expose remotely 37 | local_topics: [ 38 | ['/test1', 'std_msgs/String'], 39 | ['/closest_point', 'sensor_msgs/LaserScan'] 40 | ] 41 | # Services running in the robot to expose locally 42 | remote_services: [ 43 | ['/rosout/get_loggers', 'roscpp/GetLoggers'] 44 | ] 45 | # Services running locally to expose to the robot 46 | local_services: [ 47 | ['/add_two_ints', 'beginner_tutorials/AddTwoInts'] 48 | ] 49 | # Parameters to be sync, they will be polled to stay in sync 50 | parameters: ['/robot_description'] 51 | parameter_polling_hz: 1''' 52 | 53 | 54 | class ROSduct(object): 55 | def __init__(self): 56 | # ROSbridge 57 | self.rosbridge_ip = rospy.get_param('~rosbridge_ip', None) 58 | if self.rosbridge_ip is None: 59 | rospy.logerr('No rosbridge_ip given.') 60 | raise Exception('No rosbridge_ip given.') 61 | self.rosbridge_port = rospy.get_param('~rosbridge_port', 9090) 62 | rospy.loginfo("Will connect to ROSBridge websocket: ws://{}:{}".format( 63 | self.rosbridge_ip, self.rosbridge_port)) 64 | 65 | # Topics 66 | # TODO: check if topic types are installed, if not, give a warning 67 | self.remote_topics = rospy.get_param('~remote_topics', []) 68 | rospy.loginfo("Remote topics: " + str(self.remote_topics)) 69 | self.local_topics = rospy.get_param('~local_topics', []) 70 | rospy.loginfo("Local topics: " + str(self.local_topics)) 71 | 72 | # Services 73 | # TODO: check if service types are installed 74 | self.remote_services = rospy.get_param('~remote_services', []) 75 | rospy.loginfo("Remote services: " + str(self.remote_services)) 76 | self.local_services = rospy.get_param('~local_services', []) 77 | rospy.loginfo("Local services: " + str(self.local_services)) 78 | 79 | # Parameters 80 | self.rate_hz = rospy.get_param('~parameter_polling_hz', 1) 81 | self.parameters = rospy.get_param('~parameters', []) 82 | rospy.loginfo("Parameters: " + str(self.parameters)) 83 | self.last_params = {} 84 | 85 | if rospy.get_param("~automatic_topic_scan", "True") == "True": 86 | self.automatic_scanning = True 87 | else: 88 | self.automatic_scanning = False 89 | 90 | self.check_if_msgs_are_installed() 91 | 92 | self.initialize() 93 | 94 | def new_remote_topics(self): 95 | for r_t in self.remote_topics: 96 | self.new_remote_topic(r_t) 97 | 98 | def new_remote_topic(self, r_t): 99 | if 1: 100 | if len(r_t) == 2: 101 | topic_name, topic_type = r_t 102 | local_name = topic_name 103 | elif len(r_t) == 3: 104 | topic_name, topic_type, local_name = r_t 105 | rospub = rospy.Publisher(local_name, 106 | get_ROS_class(topic_type), 107 | # SubscribeListener added later 108 | queue_size=1) 109 | 110 | cb_r_to_l = self.create_callback_from_remote_to_local(topic_name, 111 | topic_type, 112 | rospub) 113 | subl = self.create_subscribe_listener(topic_name, 114 | topic_type, 115 | cb_r_to_l) 116 | rospub.impl.add_subscriber_listener(subl) 117 | self._instances['topics'].append( 118 | {topic_name: 119 | {'rospub': rospub, 120 | 'bridgesub': None} 121 | }) 122 | 123 | def new_local_topics(self): 124 | for l_t in self.local_topics: 125 | self.new_local_topic(l_t) 126 | 127 | def new_local_topic(self, l_t): 128 | if 1: 129 | if len(l_t) == 2: 130 | topic_name, topic_type = l_t 131 | remote_name = topic_name 132 | elif len(l_t) == 3: 133 | topic_name, topic_type, remote_name = l_t 134 | 135 | bridgepub = self.client.publisher(remote_name, topic_type) 136 | 137 | cb_l_to_r = self.create_callback_from_local_to_remote(topic_name, 138 | topic_type, 139 | bridgepub) 140 | 141 | rossub = rospy.Subscriber(topic_name, 142 | get_ROS_class(topic_type), 143 | cb_l_to_r) 144 | self._instances['topics'].append( 145 | {topic_name: 146 | {'rossub': rossub, 147 | 'bridgepub': bridgepub} 148 | }) 149 | 150 | def new_remote_services(self): 151 | for r_s in self.remote_services: 152 | self.new_remote_service(r_s) 153 | 154 | def new_remote_service(self, r_s): 155 | if 1: 156 | if len(r_s) == 2: 157 | service_name, service_type = r_s 158 | local_name = service_name 159 | elif len(r_s) == 3: 160 | service_name, service_type, local_name = r_s 161 | remote_service_client = self.client.service_client(service_name, 162 | service_type) 163 | r_to_l_serv_cb = self.create_callback_from_remote_to_local_srv( 164 | remote_service_client, 165 | service_name, 166 | service_type) 167 | rosserv = rospy.Service(local_name, 168 | get_ROS_class(service_type, 169 | srv=True), 170 | r_to_l_serv_cb) 171 | 172 | self._instances['services'].append( 173 | {service_name: 174 | {'rosserv': rosserv, 175 | 'bridgeservclient': remote_service_client} 176 | }) 177 | 178 | def new_local_services(self): 179 | for l_s in self.local_services: 180 | self.new_local_service(l_s) 181 | 182 | def new_local_service(self, l_s): 183 | if 1: 184 | if len(l_s) == 2: 185 | service_name, service_type = l_s 186 | remote_name = service_name 187 | elif len(l_s) == 3: 188 | service_name, service_type, remote_name = l_s 189 | rosservprox = rospy.ServiceProxy(service_name, 190 | get_ROS_class(service_type, 191 | srv=True)) 192 | l_to_r_srv_cv = self.create_callback_from_local_to_remote_srv( 193 | service_name, 194 | service_type, 195 | rosservprox) 196 | remote_service_server = self.client.service_server(remote_name, 197 | service_type, 198 | l_to_r_srv_cv) 199 | self._instances['services'].append( 200 | {service_name: 201 | {'rosservprox': rosservprox, 202 | 'bridgeservserver': remote_service_server} 203 | }) 204 | 205 | def initialize(self): 206 | """ 207 | Initialize creating all necessary bridged clients and servers. 208 | """ 209 | connected = False 210 | while not rospy.is_shutdown() and not connected: 211 | try: 212 | self.client = ROSBridgeClient( 213 | self.rosbridge_ip, self.rosbridge_port) 214 | connected = True 215 | except socket.error as e: 216 | rospy.logwarn( 217 | 'Error when opening websocket, is ROSBridge running?') 218 | rospy.logwarn(e) 219 | rospy.sleep(5.0) 220 | 221 | # We keep track of the instanced stuff in this dict 222 | self._instances = {'topics': [], 223 | 'services': []} 224 | 225 | self.new_remote_topics() 226 | self.new_local_topics() 227 | self.new_remote_services() 228 | self.new_local_services() 229 | 230 | # Get all params and store them for further updates 231 | for param in self.parameters: 232 | if type(param) == list: 233 | # remote param name is the first one 234 | param = param[0] 235 | self.last_params[param] = self.client.get_param(param) 236 | 237 | def create_callback_from_remote_to_local(self, topic_name, 238 | topic_type, 239 | rospub): 240 | # Note: argument MUST be named 'message' as 241 | # that's the keyword given to pydispatch 242 | def callback_remote_to_local(message): 243 | print("Remote ROSBridge subscriber from topic " + 244 | topic_name + ' of type ' + 245 | topic_type + ' got data: ' + 246 | ' which is republished locally.') 247 | # Only convert and publish with subscribers 248 | if rospub.get_num_connections() >= 1: 249 | msg = from_dict_to_ROS(message, topic_type) 250 | #try: 251 | rospub.publish(msg) 252 | #except Exception as e: 253 | # print("published" + str(e)) 254 | return callback_remote_to_local 255 | 256 | def create_callback_from_local_to_remote(self, 257 | topic_name, 258 | topic_type, 259 | bridgepub): 260 | def callback_local_to_remote(message): 261 | rospy.logdebug("Local subscriber from topic " + 262 | topic_name + ' of type ' + 263 | topic_type + ' got data: ' + str(message) + 264 | ' which is republished remotely.') 265 | dict_msg = from_ROS_to_dict(message) 266 | bridgepub.publish(dict_msg) 267 | return callback_local_to_remote 268 | 269 | def create_subscribe_listener(self, 270 | topic_name, 271 | topic_type, 272 | cb_r_to_l): 273 | # We create a SubscribeListener that will 274 | # create a rosbridge subscriber on demand 275 | # and also unregister it if no one is listening 276 | class CustomSubscribeListener(rospy.SubscribeListener): 277 | def __init__(this): 278 | super(CustomSubscribeListener, this).__init__() 279 | this.bridgesub = None 280 | 281 | def peer_subscribe(this, tn, tp, pp): 282 | # Only make a new subscriber if there wasn't one 283 | if this.bridgesub is None: 284 | rospy.logdebug( 285 | "We have a first subscriber to: " + topic_name) 286 | this.bridgesub = self.client.subscriber( 287 | topic_name, 288 | topic_type, 289 | cb_r_to_l) 290 | for idx, topic_d in enumerate(self._instances['topics']): 291 | if topic_d.get(topic_name): 292 | self._instances['topics'][idx][topic_name]['bridgesub'] = this.bridgesub 293 | break 294 | 295 | def peer_unsubscribe(this, tn, num_peers): 296 | # Unsubscribe if there isnt anyone left 297 | if num_peers < 1: 298 | rospy.logdebug( 299 | "There are no more subscribers to: " + topic_name) 300 | self.client.unsubscribe(this.bridgesub) 301 | this.bridgesub = None 302 | # May be redundant if it's actually a reference to this.bridgesub already 303 | for idx, topic_d in enumerate(self._instances['topics']): 304 | if topic_d.get(topic_name): 305 | self._instances['topics'][idx][topic_name]['bridgesub'] = None 306 | break 307 | return CustomSubscribeListener() 308 | 309 | def create_callback_from_remote_to_local_srv(self, 310 | remote_service_client, 311 | service_name, 312 | service_type): 313 | def callback_from_local_srv_call(request): 314 | rospy.loginfo("Got a SRV call to " + service_name + 315 | " of type " + service_type) 316 | req_dict = from_ROS_to_dict(request) 317 | 318 | result = { 319 | 'responded': False, 320 | 'response': None 321 | } 322 | 323 | def cb(success, response): 324 | result['responded'] = True 325 | if success: 326 | result['response'] = response 327 | remote_service_client.request(req_dict, cb) 328 | while not rospy.is_shutdown() and not result['responded']: 329 | rospy.sleep(0.1) 330 | if result['response'] is None: 331 | rospy.logerr('Service call didnt succeed (' + str(service_name) + 332 | ' of type ' + str(service_type)) 333 | return None 334 | return from_dict_to_ROS(result['response'], 335 | service_type + 'Response', 336 | srv=True) 337 | return callback_from_local_srv_call 338 | 339 | def create_callback_from_local_to_remote_srv(self, 340 | service_name, 341 | service_type, 342 | rosservprox): 343 | 344 | def callback_from_remote_service_call(request): 345 | ros_req = from_dict_to_ROS(request, service_type + 'Request', 346 | srv=True) 347 | rospy.loginfo("Waiting for server " + service_name + "...") 348 | rospy.wait_for_service(service_name) 349 | # TODO: error handling in services... 350 | resp = rosservprox.call(ros_req) 351 | resp_dict = from_ROS_to_dict(resp) 352 | return True, resp_dict 353 | 354 | return callback_from_remote_service_call 355 | 356 | def check_if_msgs_are_installed(self): 357 | """ 358 | Check if the provided message types are installed. 359 | """ 360 | for rt in self.remote_topics: 361 | if len(rt) == 2: 362 | _, topic_type = rt 363 | elif len(rt) == 3: 364 | _, topic_type, _ = rt 365 | if not is_ros_message_installed(topic_type): 366 | rospy.logwarn( 367 | "{} could not be found in the system.".format(topic_type)) 368 | 369 | for lt in self.local_topics: 370 | if len(lt) == 2: 371 | _, topic_type = lt 372 | elif len(lt) == 3: 373 | _, topic_type, _ = lt 374 | if not is_ros_message_installed(topic_type): 375 | rospy.logwarn( 376 | "{} could not be found in the system.".format(topic_type)) 377 | 378 | for rs in self.remote_services: 379 | if len(rs) == 2: 380 | _, service_type = rs 381 | elif len(rs) == 3: 382 | _, service_type, _ = rs 383 | if not is_ros_service_installed(service_type): 384 | rospy.logwarn( 385 | "{} could not be found in the system.".format(service_type)) 386 | 387 | for ls in self.local_services: 388 | if len(ls) == 2: 389 | _, service_type = ls 390 | elif len(ls) == 3: 391 | _, service_type, _ = ls 392 | if not is_ros_service_installed(service_type): 393 | rospy.logwarn( 394 | "{} could not be found in the system.".format(service_type)) 395 | 396 | def sync_params(self): 397 | """ 398 | Sync parameter server in between 399 | external and local roscore (local changes 400 | are not forwarded). 401 | """ 402 | for param in self.parameters: 403 | if type(param) == list: 404 | local_param = param[1] 405 | param = param[0] 406 | else: 407 | local_param = param 408 | # Get remote param 409 | remote_param = self.client.get_param(param) 410 | if remote_param != self.last_params[param]: 411 | rospy.set_param(local_param, remote_param) 412 | self.last_params[param] = remote_param 413 | 414 | def sync_topics(self): 415 | current_remote_topics = self.client.get_topics() 416 | current_local_topic_with_types = rospy.get_published_topics() 417 | current_local_topics = [topic[0] for topic in current_local_topic_with_types] 418 | add_to_local_topics = set(current_remote_topics) - set(current_local_topics) 419 | add_to_remote_topics = set(current_local_topics) - set(current_remote_topics) 420 | print("Add to local topics", add_to_local_topics) 421 | print("Add to remote topics", add_to_remote_topics) 422 | 423 | # add to local from remote topic 424 | for r_t in add_to_local_topics: 425 | topic_type = self.client.get_topic_type(r_t) 426 | self.new_remote_topic([r_t, topic_type]) 427 | 428 | # add to remote from local topic 429 | for l_t in add_to_remote_topics: 430 | for n, t in current_local_topic_with_types: 431 | if n == l_t: 432 | topic_type = t 433 | self.new_local_topic([l_t, topic_type]) 434 | 435 | 436 | def sync_topics(self): 437 | current_remote_topics = self.client.get_topics() 438 | current_local_topic_with_types = rospy.get_published_topics() 439 | current_local_topics = [topic[0] for topic in current_local_topic_with_types] 440 | add_to_local_topics = set(current_remote_topics) - set(current_local_topics) 441 | add_to_remote_topics = set(current_local_topics) - set(current_remote_topics) 442 | #print("Add to local topics", add_to_local_topics) 443 | #print("Add to remote topics", add_to_remote_topics) 444 | 445 | # add to local from remote topic 446 | for r_t in add_to_local_topics: 447 | topic_type = self.client.get_topic_type(r_t) 448 | self.new_remote_topic([r_t, topic_type]) 449 | 450 | # add to remote from local topic 451 | for l_t in add_to_remote_topics: 452 | for n, t in current_local_topic_with_types: 453 | if n == l_t: 454 | topic_type = t 455 | self.new_local_topic([l_t, topic_type]) 456 | 457 | def sync_topics(self): 458 | current_remote_topics = self.client.get_topics() 459 | current_local_topic_with_types = rospy.get_published_topics() 460 | current_local_topics = [topic[0] for topic in current_local_topic_with_types] 461 | add_to_local_topics = set(current_remote_topics) - set(current_local_topics) 462 | add_to_remote_topics = set(current_local_topics) - set(current_remote_topics) 463 | #print("Add to local topics", add_to_local_topics) 464 | #print("Add to remote topics", add_to_remote_topics) 465 | 466 | # add to local from remote topic 467 | for r_t in add_to_local_topics: 468 | topic_type = self.client.get_topic_type(r_t) 469 | self.new_remote_topic([r_t, topic_type]) 470 | 471 | # add to remote from local topic 472 | for l_t in add_to_remote_topics: 473 | for n, t in current_local_topic_with_types: 474 | if n == l_t: 475 | topic_type = t 476 | self.new_local_topic([l_t, topic_type]) 477 | 478 | def sync_topics(self): 479 | current_remote_topics = self.client.get_topics() 480 | current_local_topic_with_types = rospy.get_published_topics() 481 | current_local_topics = [topic[0] for topic in current_local_topic_with_types] 482 | add_to_local_topics = set(current_remote_topics) - set(current_local_topics) 483 | add_to_remote_topics = set(current_local_topics) - set(current_remote_topics) 484 | #print("Add to local topics", add_to_local_topics) 485 | #print("Add to remote topics", add_to_remote_topics) 486 | 487 | # add to local from remote topic 488 | for r_t in add_to_local_topics: 489 | topic_type = self.client.get_topic_type(r_t) 490 | self.new_remote_topic([r_t, topic_type]) 491 | 492 | # add to remote from local topic 493 | for l_t in add_to_remote_topics: 494 | for n, t in current_local_topic_with_types: 495 | if n == l_t: 496 | topic_type = t 497 | self.new_local_topic([l_t, topic_type]) 498 | 499 | def sync_services(self): 500 | import rosservice 501 | current_remote_services = [service for service in self.client.get_services() if not service.startswith("/ros")] 502 | current_local_services = [service for service in rosservice.get_service_list() if not service.startswith("/ros")] 503 | #print(current_remote_services) 504 | #print(current_local_services) 505 | add_to_local_services = set(current_remote_services) - set(current_local_services) 506 | add_to_remote_services = set(current_local_services) - set(current_remote_services) 507 | #print("Add to local services", add_to_local_services) 508 | #print("Add to remote services", add_to_remote_services) 509 | 510 | # add to local from remote service 511 | for r_s in add_to_local_services: 512 | service_type = self.client.get_service_type(r_s) 513 | self.new_remote_service([r_s, service_type]) 514 | 515 | # add to remote from local service 516 | for l_s in add_to_remote_services: 517 | #for n, t in current_local_service_with_types: 518 | # if n == l_t: 519 | # service_type = t 520 | service_type = rosservice.get_service_type(l_s) 521 | self.new_local_service([l_s, service_type]) 522 | 523 | 524 | def spin(self): 525 | """ 526 | Run the node, needed to update the parameter server. 527 | """ 528 | r = rospy.Rate(self.rate_hz) 529 | while not rospy.is_shutdown(): 530 | self.sync_params() 531 | if self.automatic_scanning: 532 | self.sync_topics() 533 | self.sync_services() 534 | r.sleep() 535 | 536 | 537 | if __name__ == '__main__': 538 | rospy.init_node('rosduct') 539 | r = ROSduct() 540 | r.spin() 541 | -------------------------------------------------------------------------------- /srv/AddTwoInts.srv: -------------------------------------------------------------------------------- 1 | int64 a 2 | int64 b 3 | --- 4 | int64 sum -------------------------------------------------------------------------------- /srv/beginner_tutorials.srv: -------------------------------------------------------------------------------- 1 | int64 a 2 | int64 b 3 | --- 4 | int64 sum --------------------------------------------------------------------------------