├── .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 | 
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 | 
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
--------------------------------------------------------------------------------