├── spot_ros_msgs ├── msg │ ├── FootState.msg │ ├── BehaviorFaultState.msg │ ├── ImageCapture.msg │ ├── CommsState.msg │ ├── EStopState.msg │ ├── SystemFault.msg │ ├── SystemFaultState.msg │ ├── PowerState.msg │ ├── RobotState.msg │ └── KinematicState.msg ├── CMakeLists.txt └── package.xml ├── install_script ├── requirements.txt └── install_dependencies.sh ├── docs ├── StartingVcXsrv.png └── SystemArchitecture.png ├── spot_ros_srvs ├── srv │ ├── Trajectory.srv │ ├── Velocity.srv │ └── Stand.srv ├── CMakeLists.txt └── package.xml ├── spot_ros_interface ├── setup.py ├── CMakeLists.txt ├── package.xml └── scripts │ ├── test │ ├── stand_srv_caller.py │ └── dummy_joint_state_pub.py │ ├── tf_remapper.py │ ├── keyboard_teleop.py │ ├── grid_utils.py │ └── spot_ros_interface.py ├── spot_urdf ├── launch │ └── rviz_display.launch ├── package.xml ├── rviz │ ├── urdf.rviz │ └── occupancy_grid_config.rviz └── CMakeLists.txt ├── LICENSE.md ├── .gitignore ├── SECURITY.md └── README.md /spot_ros_msgs/msg/FootState.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Vector3 foot_position_rt_body 2 | uint8 contact -------------------------------------------------------------------------------- /spot_ros_msgs/msg/BehaviorFaultState.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | uint8 cause 3 | uint8 status -------------------------------------------------------------------------------- /install_script/requirements.txt: -------------------------------------------------------------------------------- 1 | catkin_tools 2 | pyyaml 3 | rospkg 4 | PyQt5 5 | pydot 6 | readchar 7 | -------------------------------------------------------------------------------- /docs/StartingVcXsrv.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/microsoft/spot-ros-wrapper/HEAD/docs/StartingVcXsrv.png -------------------------------------------------------------------------------- /spot_ros_msgs/msg/ImageCapture.msg: -------------------------------------------------------------------------------- 1 | sensor_msgs/Image image 2 | geometry_msgs/Transform ko_tform_body 3 | -------------------------------------------------------------------------------- /spot_ros_srvs/srv/Trajectory.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/PoseArray waypoints 2 | --- 3 | geometry_msgs/Pose final_pose -------------------------------------------------------------------------------- /spot_ros_srvs/srv/Velocity.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/Twist velocity # Only x, y, and angular_z will be used 2 | --- -------------------------------------------------------------------------------- /docs/SystemArchitecture.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/microsoft/spot-ros-wrapper/HEAD/docs/SystemArchitecture.png -------------------------------------------------------------------------------- /spot_ros_msgs/msg/CommsState.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | uint8 wifi_mode #[WiFiState.Mode enum] 3 | string essid -------------------------------------------------------------------------------- /spot_ros_msgs/msg/EStopState.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | uint8 type 3 | uint8 state 4 | string state_description -------------------------------------------------------------------------------- /spot_ros_srvs/srv/Stand.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/Transform body_pose # Only the z-component of translation will be taken into account 2 | --- -------------------------------------------------------------------------------- /spot_ros_msgs/msg/SystemFault.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | time duration 3 | int32 code 4 | uint64 uid 5 | string error_message 6 | string attributes 7 | uint8 severity -------------------------------------------------------------------------------- /spot_ros_msgs/msg/SystemFaultState.msg: -------------------------------------------------------------------------------- 1 | SystemFault[] faults 2 | SystemFault[] historical_faults 3 | diagnostic_msgs/KeyValue[] aggregated #[SystemFaultState.AggregatedEntry] -------------------------------------------------------------------------------- /spot_ros_msgs/msg/PowerState.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | uint8 motor_power_state #[enum] 3 | uint8 shore_power_state #[enum] 4 | float32 locomotion_charge_percentage 5 | time locomotion_estimated_runtime -------------------------------------------------------------------------------- /spot_ros_msgs/msg/RobotState.msg: -------------------------------------------------------------------------------- 1 | PowerState power_state 2 | sensor_msgs/BatteryState[] battery_states 3 | CommsState[] comms_states 4 | SystemFaultState system_fault_state 5 | EStopState[] estop_states 6 | BehaviorFaultState[] behavior_fault_states 7 | FootState[] foot_states -------------------------------------------------------------------------------- /spot_ros_msgs/msg/KinematicState.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | sensor_msgs/JointState joint_states 3 | geometry_msgs/Transform vision_tform_body 4 | geometry_msgs/Transform odom_tform_body 5 | geometry_msgs/Twist velocity_of_body_in_vision 6 | geometry_msgs/Twist velocity_of_body_in_odom -------------------------------------------------------------------------------- /spot_ros_interface/setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup 2 | from catkin_pkg.python_setup import generate_distutils_setup 3 | 4 | d = generate_distutils_setup( 5 | packages=['spot_ros_interface'], 6 | scripts=['scripts/spot_ros_interface.py'], 7 | package_dir={'': 'src'} 8 | ) 9 | 10 | setup(**d) -------------------------------------------------------------------------------- /spot_ros_interface/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(spot_ros_interface) 3 | 4 | ## Find catkin and any catkin packages 5 | find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs genmsg spot_ros_msgs cv_bridge) 6 | 7 | ## Declare a catkin package 8 | catkin_package() 9 | 10 | include_directories(include ${catkin_INCLUDE_DIRS}) 11 | 12 | catkin_python_setup() -------------------------------------------------------------------------------- /install_script/install_dependencies.sh: -------------------------------------------------------------------------------- 1 | echo "Updating apt-get" 2 | sudo apt update 3 | sudo apt-get update 4 | 5 | echo "Installing system packages" 6 | sudo apt-get -y install python3-pip 7 | sudo apt-get install python3-catkin-tools 8 | 9 | sudo apt-get install ros-noetic-rosbridge-server 10 | sudo apt-get install python-is-python3 #to point "python" to "python3" 11 | 12 | echo "Installing pip packages" 13 | pip install -r ./requirements.txt 14 | -------------------------------------------------------------------------------- /spot_urdf/launch/rviz_display.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | ["joint_state_from_spot"] 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /spot_ros_srvs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(spot_ros_srvs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | std_msgs 6 | geometry_msgs 7 | sensor_msgs 8 | message_generation 9 | diagnostic_msgs 10 | spot_ros_msgs 11 | ) 12 | 13 | ## Generate services in the 'srv' folder 14 | add_service_files( 15 | FILES 16 | Stand.srv 17 | Trajectory.srv 18 | Velocity.srv 19 | ) 20 | 21 | ## Generate added messages and services with any dependencies listed here 22 | generate_messages( 23 | DEPENDENCIES 24 | std_msgs 25 | geometry_msgs 26 | sensor_msgs 27 | diagnostic_msgs 28 | spot_ros_msgs 29 | ) 30 | 31 | include_directories( 32 | ${catkin_INCLUDE_DIRS} 33 | ) 34 | 35 | catkin_package( 36 | CATKIN_DEPENDS message_runtime std_msgs geometry_msgs sensor_msgs spot_ros_msgs 37 | ) -------------------------------------------------------------------------------- /spot_ros_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(spot_ros_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | std_msgs 6 | geometry_msgs 7 | sensor_msgs 8 | diagnostic_msgs 9 | message_generation 10 | ) 11 | 12 | ## Generate messages in the 'msg' folder 13 | add_message_files( 14 | FILES 15 | KinematicState.msg 16 | ImageCapture.msg 17 | PowerState.msg 18 | CommsState.msg 19 | SystemFault.msg 20 | SystemFaultState.msg 21 | EStopState.msg 22 | BehaviorFaultState.msg 23 | FootState.msg 24 | RobotState.msg 25 | ) 26 | 27 | ## Generate added messages and services with any dependencies listed here 28 | generate_messages( 29 | DEPENDENCIES 30 | std_msgs 31 | geometry_msgs 32 | sensor_msgs 33 | diagnostic_msgs 34 | ) 35 | 36 | include_directories( 37 | ${catkin_INCLUDE_DIRS} 38 | ) 39 | 40 | catkin_package( 41 | CATKIN_DEPENDS message_runtime std_msgs geometry_msgs sensor_msgs 42 | ) -------------------------------------------------------------------------------- /spot_ros_interface/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | spot_ros_interface 4 | 0.0.0 5 | 6 | ROS wrapper for the Spot SDK (https://github.com/boston-dynamics/spot-sdk) 7 | that exposes some key capabilities via standard ROS messages 8 | 9 | t-oferro 10 | TODO 11 | 12 | 13 | catkin 14 | 15 | rospy 16 | rospy 17 | rospy 18 | 19 | std_msgs 20 | std_msgs 21 | std_msgs 22 | 23 | spot_ros_msgs 24 | spot_ros_msgs 25 | spot_ros_msgs 26 | 27 | 28 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Microsoft 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /spot_ros_srvs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | spot_ros_srvs 4 | 0.0.0 5 | The spot_ros_srvs package 6 | t-oferro 7 | TODO 8 | 9 | 10 | catkin 11 | 12 | 13 | message_generation 14 | message_runtime 15 | 16 | std_msgs 17 | std_msgs 18 | std_msgs 19 | 20 | geometry_msgs 21 | geometry_msgs 22 | geometry_msgs 23 | 24 | sensor_msgs 25 | sensor_msgs 26 | sensor_msgs 27 | 28 | spot_ros_msgs 29 | spot_ros_msgs 30 | spot_ros_msgs 31 | 32 | 33 | -------------------------------------------------------------------------------- /spot_ros_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | spot_ros_msgs 4 | 0.0.0 5 | The spot_ros_msgs package 6 | t-oferro 7 | TODO 8 | 9 | 10 | 11 | catkin 12 | 13 | message_generation 14 | message_runtime 15 | 16 | std_msgs 17 | std_msgs 18 | std_msgs 19 | 20 | geometry_msgs 21 | geometry_msgs 22 | geometry_msgs 23 | 24 | sensor_msgs 25 | sensor_msgs 26 | sensor_msgs 27 | 28 | diagnostic_msgs 29 | diagnostic_msgs 30 | diagnostic_msgs 31 | 32 | 33 | -------------------------------------------------------------------------------- /spot_ros_interface/scripts/test/stand_srv_caller.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | ''' 4 | Test script that calls the stand service from the spot_ros_interface at 0.5 Hz. 5 | Moves Spot in a sinusoidal shape up and down. 6 | 7 | Note: spot_ros_interface must be running in order to accept service calls. 8 | ''' 9 | 10 | import rospy 11 | import geometry_msgs.msg 12 | import spot_ros_srvs.srv 13 | 14 | import time 15 | import math 16 | 17 | # Initialize ROS node 18 | rospy.init_node('stand_srv_caller') 19 | rate = rospy.Rate(2) 20 | 21 | # Declare stnad service proxy 22 | stand_srv_pub = rospy.ServiceProxy("stand_cmd", spot_ros_srvs.srv.Stand) 23 | 24 | tf = geometry_msgs.msg.Transform() 25 | srv_req = spot_ros_srvs.srv.StandRequest() 26 | 27 | 28 | start_time = time.time() 29 | 30 | while not rospy.is_shutdown(): 31 | 32 | # Set target height to +/- 0.15 m from the resting height 33 | tf.translation.z = 0.15*math.sin((time.time()-start_time)%(2*math.pi)) 34 | 35 | # Construct service request 36 | srv_req.body_pose = tf 37 | 38 | # Call service, if available 39 | try: 40 | rospy.wait_for_service("stand_cmd", timeout=0.5) 41 | stand_srv_pub(srv_req) 42 | except rospy.ServiceException as e: 43 | print("Service call failed: %s"%e) 44 | print("Height: {}".format(tf.translation.z)) 45 | 46 | rate.sleep() -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | *.egg-info/ 24 | .installed.cfg 25 | *.egg 26 | MANIFEST 27 | 28 | # PyInstaller 29 | # Usually these files are written by a python script from a template 30 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 31 | *.manifest 32 | *.spec 33 | 34 | # Installer logs 35 | pip-log.txt 36 | pip-delete-this-directory.txt 37 | 38 | # Unit test / coverage reports 39 | htmlcov/ 40 | .tox/ 41 | .coverage 42 | .coverage.* 43 | .cache 44 | nosetests.xml 45 | coverage.xml 46 | *.cover 47 | .hypothesis/ 48 | .pytest_cache/ 49 | 50 | # Translations 51 | *.mo 52 | *.pot 53 | 54 | # Django stuff: 55 | *.log 56 | local_settings.py 57 | db.sqlite3 58 | 59 | # Flask stuff: 60 | instance/ 61 | .webassets-cache 62 | 63 | # Scrapy stuff: 64 | .scrapy 65 | 66 | # Sphinx documentation 67 | docs/_build/ 68 | 69 | # PyBuilder 70 | target/ 71 | 72 | # Jupyter Notebook 73 | .ipynb_checkpoints 74 | 75 | # pyenv 76 | .python-version 77 | 78 | # celery beat schedule file 79 | celerybeat-schedule 80 | 81 | # SageMath parsed files 82 | *.sage.py 83 | 84 | # Environments 85 | .env 86 | .venv 87 | env/ 88 | venv/ 89 | ENV/ 90 | env.bak/ 91 | venv.bak/ 92 | 93 | # Spyder project settings 94 | .spyderproject 95 | .spyproject 96 | 97 | # Rope project settings 98 | .ropeproject 99 | 100 | # mkdocs documentation 101 | /site 102 | 103 | # mypy 104 | .mypy_cache/ 105 | 106 | .vscode 107 | /.vs 108 | 109 | .catkin_tools 110 | devel/ 111 | build/ 112 | logs/ 113 | -------------------------------------------------------------------------------- /spot_urdf/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | spot_urdf 4 | 0.0.0 5 | The spot_urdf package 6 | 7 | 8 | 9 | 10 | t-oferro 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 | joint_state_publisher_gui 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /SECURITY.md: -------------------------------------------------------------------------------- 1 | 2 | 3 | ## Security 4 | 5 | Microsoft takes the security of our software products and services seriously, which includes all source code repositories managed through our GitHub organizations, which include [Microsoft](https://github.com/Microsoft), [Azure](https://github.com/Azure), [DotNet](https://github.com/dotnet), [AspNet](https://github.com/aspnet), [Xamarin](https://github.com/xamarin), and [our GitHub organizations](https://opensource.microsoft.com/). 6 | 7 | If you believe you have found a security vulnerability in any Microsoft-owned repository that meets [Microsoft's definition of a security vulnerability](https://aka.ms/opensource/security/definition), please report it to us as described below. 8 | 9 | ## Reporting Security Issues 10 | 11 | **Please do not report security vulnerabilities through public GitHub issues.** 12 | 13 | Instead, please report them to the Microsoft Security Response Center (MSRC) at [https://msrc.microsoft.com/create-report](https://aka.ms/opensource/security/create-report). 14 | 15 | If you prefer to submit without logging in, send email to [secure@microsoft.com](mailto:secure@microsoft.com). If possible, encrypt your message with our PGP key; please download it from the [Microsoft Security Response Center PGP Key page](https://aka.ms/opensource/security/pgpkey). 16 | 17 | You should receive a response within 24 hours. If for some reason you do not, please follow up via email to ensure we received your original message. Additional information can be found at [microsoft.com/msrc](https://aka.ms/opensource/security/msrc). 18 | 19 | Please include the requested information listed below (as much as you can provide) to help us better understand the nature and scope of the possible issue: 20 | 21 | * Type of issue (e.g. buffer overflow, SQL injection, cross-site scripting, etc.) 22 | * Full paths of source file(s) related to the manifestation of the issue 23 | * The location of the affected source code (tag/branch/commit or direct URL) 24 | * Any special configuration required to reproduce the issue 25 | * Step-by-step instructions to reproduce the issue 26 | * Proof-of-concept or exploit code (if possible) 27 | * Impact of the issue, including how an attacker might exploit the issue 28 | 29 | This information will help us triage your report more quickly. 30 | 31 | If you are reporting for a bug bounty, more complete reports can contribute to a higher bounty award. Please visit our [Microsoft Bug Bounty Program](https://aka.ms/opensource/security/bounty) page for more details about our active programs. 32 | 33 | ## Preferred Languages 34 | 35 | We prefer all communications to be in English. 36 | 37 | ## Policy 38 | 39 | Microsoft follows the principle of [Coordinated Vulnerability Disclosure](https://aka.ms/opensource/security/cvd). 40 | 41 | 42 | -------------------------------------------------------------------------------- /spot_ros_interface/scripts/tf_remapper.py: -------------------------------------------------------------------------------- 1 | # import rospy 2 | # import tf2_ros 3 | # import geometry_msgs.msg 4 | 5 | # unity_cam_broadcaster = tf2_ros.TransformBroadcaster() 6 | 7 | # t = geometry_msgs.msg.TransformStamped() 8 | # t.header.stamp = rospy.Time.now() 9 | # t.header.frame_id = "vision_odometry_frame" 10 | # t.child_frame_id = "base_link" 11 | # t.transform.translation.x = kinematic_state.vision_tform_body.translation.x 12 | # t.transform.translation.y = kinematic_state.vision_tform_body.translation.y 13 | # t.transform.translation.z = kinematic_state.vision_tform_body.translation.z 14 | # t.transform.rotation.x = kinematic_state.vision_tform_body.rotation.x 15 | # t.transform.rotation.y = kinematic_state.vision_tform_body.rotation.y 16 | # t.transform.rotation.z = kinematic_state.vision_tform_body.rotation.z 17 | # t.transform.rotation.w = kinematic_state.vision_tform_body.rotation.w 18 | 19 | # unity_cam_broadcaster.sendTransform(t) 20 | 21 | import rospy 22 | import math 23 | import tf 24 | import tf2_ros 25 | import tf2_msgs.msg 26 | import geometry_msgs.msg 27 | 28 | t = geometry_msgs.msg.TransformStamped() 29 | t.header.frame_id = "7e62ccfd-eb9a-46f8-a221-acbbcb906838" 30 | t.child_frame_id = "unity_cam" 31 | t.transform.translation.x = 1 32 | t.transform.translation.y = 1 33 | t.transform.translation.z = 1 34 | t.transform.rotation.x = 0 35 | t.transform.rotation.y = 0 36 | t.transform.rotation.z = 0 37 | t.transform.rotation.w = 1 38 | 39 | def callback(data): 40 | rospy.loginfo("UPDATING!") 41 | 42 | t.header.frame_id = "7e62ccfd-eb9a-46f8-a221-acbbcb906838" 43 | t.child_frame_id = "unity_cam" 44 | t.transform.translation.x = data.transforms[0].transform.translation.x 45 | t.transform.translation.y = data.transforms[0].transform.translation.y 46 | t.transform.translation.z = data.transforms[0].transform.translation.z 47 | t.transform.rotation.x = data.transforms[0].transform.rotation.x 48 | t.transform.rotation.y = data.transforms[0].transform.rotation.y 49 | t.transform.rotation.z = data.transforms[0].transform.rotation.z 50 | t.transform.rotation.w = data.transforms[0].transform.rotation.w 51 | 52 | print(t) 53 | 54 | 55 | if __name__ == '__main__': 56 | rospy.init_node('tf_remapper') 57 | 58 | listener = tf.TransformListener() 59 | publisher = tf2_ros.TransformBroadcaster() 60 | msg_listener = rospy.Subscriber("unity_cam_tf", tf2_msgs.msg.TFMessage, callback) 61 | 62 | rate = rospy.Rate(60.0) 63 | while not rospy.is_shutdown(): 64 | # try: 65 | # trans, rot = listener.lookupTransform('/unity_cam', '/7e62ccfd-eb9a-46f8-a221-acbbcb906838', rospy.Time(0)) #('/base_link', '/vision_odometry_frame', rospy.Time(0)) 66 | # print(trans) 67 | # print(rot) 68 | 69 | 70 | # print("SENT") 71 | # except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 72 | # continue 73 | 74 | t.header.stamp = rospy.Time.now() 75 | publisher.sendTransform(t) 76 | print(".") 77 | 78 | rate.sleep() -------------------------------------------------------------------------------- /spot_ros_interface/scripts/test/dummy_joint_state_pub.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | ''' 4 | Test script that publishes simulated joint angles to test the Rviz simulation. 5 | To visualize, run `roslaunch spot_urdf rviz_display.launch` 6 | ''' 7 | 8 | import rospy 9 | import geometry_msgs.msg 10 | import std_msgs.msg 11 | import sensor_msgs.msg 12 | import spot_ros_msgs.msg 13 | import tf2_ros 14 | import math 15 | import time 16 | 17 | # Set up joint publisher 18 | joint_state_pub = rospy.Publisher( 19 | "joint_state_from_spot", sensor_msgs.msg.JointState, queue_size=20) 20 | 21 | # RViz visualization expects a transform from Spot's `base_link` to 22 | # the world frame `vision_odometry_frame` 23 | spot_tf_broadcaster = tf2_ros.TransformBroadcaster() 24 | 25 | # Initialize ROS node 26 | rospy.init_node('dummy_spot_ros_interface_py') 27 | rate = rospy.Rate(10) # Update at 10hz 28 | 29 | 30 | start_time = time.time() 31 | while not rospy.is_shutdown(): 32 | print("Looping...") 33 | 34 | # Populate and broadcast transform from base_link to vision_odometry_frame 35 | t = geometry_msgs.msg.TransformStamped() 36 | t.header.stamp = rospy.Time.now() 37 | t.header.frame_id = "vision_odometry_frame" 38 | t.child_frame_id = "base_link" 39 | t.transform.rotation.w = 1 40 | spot_tf_broadcaster.sendTransform(t) 41 | 42 | # Populate joint state msg 43 | js = sensor_msgs.msg.JointState() 44 | js.name.append("fl.hx") 45 | js.name.append("fl.hy") 46 | js.name.append("fl.kn") 47 | js.name.append("fr.hx") 48 | js.name.append("fr.hy") 49 | js.name.append("fr.kn") 50 | js.name.append("hl.hx") 51 | js.name.append("hl.hy") 52 | js.name.append("hl.kn") 53 | js.name.append("hr.hx") 54 | js.name.append("hr.hy") 55 | js.name.append("hr.kn") 56 | 57 | # Animate joints in sinusoidal shape. Note: Not a real gait, just for testing the visualization 58 | js.position = [ 59 | 0.0 , #fl.hx 60 | 0.5*math.sin((time.time()-start_time)%(2*math.pi) + math.pi)+math.pi/4, #fl.hy 61 | -math.pi/3 + 0.5*math.sin((time.time()-start_time)%(2*math.pi)+math.pi), #fl.kn 62 | 0.0, #fr.hx 63 | 0 + 0.5*math.sin((time.time()-start_time)%(2*math.pi))+math.pi/4, #fr.hy 64 | -math.pi/2. + math.pi/6+ 0.5*math.sin((time.time()-start_time)%(2*math.pi)),#fr.kn 65 | 0.0, #hl.hx 66 | 0.5*math.sin((time.time()-start_time)%(2*math.pi))+math.pi/4, #hl.hy 67 | -math.pi/3 + 0.5*math.sin((time.time()-start_time)%(2*math.pi)), #hl.kn 68 | 0.0, #hr.hx 69 | 0.5*math.sin((time.time()-start_time)%(2*math.pi) + math.pi)+math.pi/4, #hr.hy 70 | -math.pi/3 + 0.5*math.sin((time.time()-start_time)%(2*math.pi)+math.pi) #hr.kn 71 | ] 72 | 73 | # Publish joint state 74 | joint_state_pub.publish(js) 75 | 76 | rate.sleep() 77 | -------------------------------------------------------------------------------- /spot_ros_interface/scripts/keyboard_teleop.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import rospy 3 | import geometry_msgs.msg 4 | import std_msgs.msg 5 | import sensor_msgs.msg 6 | import spot_ros_msgs.msg 7 | import spot_ros_srvs.srv 8 | 9 | import time 10 | import math 11 | import readchar 12 | import sys 13 | import os 14 | 15 | 16 | line='\u2500' 17 | instructions="\n\ 18 | \u250C{} SPOT KEYBOARD TELEOP {}\u2510 \n\ 19 | \u2502 \u2502\n\ 20 | \u2502 wasd - Move \u2502\n\ 21 | \u2502 qe - Turn \u2502\n\ 22 | \u2502 r - Self-right \u2502\n\ 23 | \u2502 j - Height up \u2502\n\ 24 | \u2502 k - Height down \u2502\n\ 25 | \u2502 \u2502\n\ 26 | \u2502 SPACE - E-Stop (TODO) \u2502\n\ 27 | \u2502 Q - Quit \u2502\n\ 28 | \u2502 \u2502\n\ 29 | \u2514{}\u2518\ 30 | ".format(line*3,line*3,line*28) 31 | 32 | # Get size of terminal window 33 | rows, columns = os.popen('stty size', 'r').read().split() 34 | 35 | # Define velocity at which Spot will move 36 | lin_vel = 1.0 # m/s 37 | ang_vel = 1.0 38 | height_up = 1.0 39 | height_down = -1.0 40 | 41 | def self_right_service(key): 42 | tf = geometry_msgs.msg.Transform() 43 | 44 | self_right_srv_req.body_pose.translation = tf.translation 45 | self_right_srv_req.body_pose.rotation = tf.rotation 46 | 47 | try: 48 | rospy.wait_for_service("self_right_cmd", timeout=2.0) 49 | self_right_srv_pub(self_right_srv_req) 50 | except rospy.ServiceException as e: 51 | print("Service call failed: %s"%e) 52 | 53 | 54 | def stand_service(key): 55 | tf = geometry_msgs.msg.Transform() 56 | 57 | if key=='j': 58 | tf.translation.z = height_up 59 | elif key=='k': 60 | tf.translation.z = height_down 61 | 62 | stand_srv_req.body_pose.translation = tf.translation 63 | stand_srv_req.body_pose.rotation = tf.rotation 64 | 65 | try: 66 | rospy.wait_for_service("stand_cmd", timeout=2.0) 67 | stand_srv_pub(stand_srv_req) 68 | except rospy.ServiceException as e: 69 | print("Service call failed: %s"%e) 70 | 71 | def vel_service(key): 72 | twist = geometry_msgs.msg.Twist() 73 | 74 | if key=='w': 75 | twist.linear.x = lin_vel 76 | elif key=='a': 77 | twist.linear.y = lin_vel 78 | elif key=='s': 79 | twist.linear.x = -lin_vel 80 | elif key=='d': 81 | twist.linear.y = -lin_vel 82 | elif key=='q': 83 | twist.angular.z = ang_vel 84 | elif key=='e': 85 | twist.angular.z = -ang_vel 86 | 87 | vel_srv_req.velocity.linear = twist.linear 88 | vel_srv_req.velocity.angular = twist.angular 89 | 90 | try: 91 | rospy.wait_for_service("velocity_cmd", timeout=2.0) 92 | vel_srv_pub(vel_srv_req) 93 | except rospy.ServiceException as e: 94 | print("Service call failed: %s"%e, end='') 95 | 96 | 97 | rospy.init_node('keyboard_teleop') 98 | rate = rospy.Rate(60) 99 | 100 | # Define service proxies 101 | self_right_srv_pub = rospy.ServiceProxy("self_right_cmd", spot_ros_srvs.srv.Stand) 102 | stand_srv_pub = rospy.ServiceProxy("stand_cmd", spot_ros_srvs.srv.Stand) 103 | vel_srv_pub = rospy.ServiceProxy("velocity_cmd", spot_ros_srvs.srv.Velocity) 104 | 105 | # Define service requests 106 | self_right_srv_req = spot_ros_srvs.srv.StandRequest() 107 | stand_srv_req = spot_ros_srvs.srv.StandRequest() 108 | vel_srv_req = spot_ros_srvs.srv.VelocityRequest() 109 | 110 | print(instructions) 111 | while not rospy.is_shutdown(): 112 | key = readchar.readkey() 113 | print('{}\rKey pressed: {}\r'.format(' '*int(columns), key), end="") 114 | 115 | if key=="Q": 116 | sys.exit() 117 | 118 | if key in 'wasdqe': 119 | vel_service(key) 120 | elif key in 'jk': 121 | stand_service(key) 122 | elif key in 'r': 123 | self_right_service(key) 124 | 125 | 126 | rate.sleep() -------------------------------------------------------------------------------- /spot_urdf/rviz/urdf.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /RobotModel1 10 | - /TF1 11 | Splitter Ratio: 0.5 12 | Tree Height: 557 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.588679 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: "" 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz/Grid 38 | Color: 160; 160; 164 39 | Enabled: true 40 | Line Style: 41 | Line Width: 0.03 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 10 51 | Reference Frame: 52 | Value: true 53 | - Alpha: 0.5 54 | Class: rviz/RobotModel 55 | Collision Enabled: false 56 | Enabled: true 57 | Links: 58 | All Links Enabled: true 59 | Expand Joint Details: false 60 | Expand Link Details: false 61 | Expand Tree: false 62 | Link Tree Style: Links in Alphabetic Order 63 | base_link: 64 | Alpha: 1 65 | Show Axes: false 66 | Show Trail: false 67 | Value: true 68 | right_leg: 69 | Alpha: 1 70 | Show Axes: false 71 | Show Trail: false 72 | Value: true 73 | Name: RobotModel 74 | Robot Description: robot_description 75 | TF Prefix: "" 76 | Update Interval: 0 77 | Value: true 78 | Visual Enabled: true 79 | - Class: rviz/TF 80 | Enabled: true 81 | Frame Timeout: 15 82 | Frames: 83 | All Enabled: true 84 | base_link: 85 | Value: true 86 | right_leg: 87 | Value: true 88 | Marker Scale: 0.5 89 | Name: TF 90 | Show Arrows: true 91 | Show Axes: true 92 | Show Names: true 93 | Tree: 94 | base_link: 95 | right_leg: 96 | {} 97 | Update Interval: 0 98 | Value: true 99 | Enabled: true 100 | Global Options: 101 | Background Color: 48; 48; 48 102 | Fixed Frame: base_link 103 | Frame Rate: 30 104 | Name: root 105 | Tools: 106 | - Class: rviz/Interact 107 | Hide Inactive Objects: true 108 | - Class: rviz/MoveCamera 109 | - Class: rviz/Select 110 | - Class: rviz/FocusCamera 111 | - Class: rviz/Measure 112 | - Class: rviz/SetInitialPose 113 | Topic: /initialpose 114 | - Class: rviz/SetGoal 115 | Topic: /move_base_simple/goal 116 | - Class: rviz/PublishPoint 117 | Single click: true 118 | Topic: /clicked_point 119 | Value: true 120 | Views: 121 | Current: 122 | Class: rviz/Orbit 123 | Distance: 4.48689 124 | Enable Stereo Rendering: 125 | Stereo Eye Separation: 0.06 126 | Stereo Focal Distance: 1 127 | Swap Stereo Eyes: false 128 | Value: false 129 | Focal Point: 130 | X: 0 131 | Y: 0 132 | Z: 0 133 | Name: Current View 134 | Near Clip Distance: 0.01 135 | Pitch: 0.695397 136 | Target Frame: 137 | Value: Orbit (rviz) 138 | Yaw: 0.513582 139 | Saved: ~ 140 | Window Geometry: 141 | Displays: 142 | collapsed: false 143 | Height: 882 144 | Hide Left Dock: false 145 | Hide Right Dock: false 146 | QMainWindow State: 000000ff00000000fd00000004000000000000013c000002c3fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000043000002c3000000de00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000043000002c3000000b800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004c000000044fc0100000002fb0000000800540069006d00650100000000000004c00000025800fffffffb0000000800540069006d0065010000000000000450000000000000000000000269000002c300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 147 | Selection: 148 | collapsed: false 149 | Time: 150 | collapsed: false 151 | Tool Properties: 152 | collapsed: false 153 | Views: 154 | collapsed: false 155 | Width: 1216 156 | X: 17 157 | Y: 28 158 | -------------------------------------------------------------------------------- /spot_ros_interface/scripts/grid_utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import math 3 | 4 | import rospy 5 | import geometry_msgs.msg 6 | import std_msgs.msg 7 | import visualization_msgs.msg 8 | 9 | from bosdyn.api import local_grid_pb2 10 | from bosdyn.client.frame_helpers import get_a_tform_b, VISION_FRAME_NAME 11 | 12 | 13 | ### Set of helper functions for local_grid processing 14 | def get_terrain_markers(local_grid_proto): 15 | '''Receives raw proto from grid_client.get_local_grids(...) and returns marker array msg''' 16 | for local_grid in local_grid_proto: 17 | if local_grid.local_grid_type_name == "terrain": #TODO: Support parsing the terrain_valid and the intensity fields in the proto 18 | vision_tform_local_grid = get_a_tform_b( 19 | local_grid.local_grid.transforms_snapshot, 20 | VISION_FRAME_NAME, 21 | local_grid.local_grid.frame_name_local_grid_data).to_proto() 22 | 23 | cell_size = local_grid.local_grid.extent.cell_size 24 | terrain_pts = get_terrain_grid(local_grid) 25 | 26 | # terrain_pts is [[x1,y1,z1], [x2,y2,z2], [x3,y3,z3], ...] (a list of lists, each of which is a point) 27 | # in the correct relative pose to Spot's body 28 | terrain_pts = offset_grid_pixels(terrain_pts, vision_tform_local_grid, cell_size) 29 | 30 | # Parse terrain_pts into Marker (cube_list) 31 | # Note: Using a single Marker of type CUBE_LIST allows for batch rendering, whereas MarkerArray of CUBEs does not 32 | marker = visualization_msgs.msg.Marker() 33 | marker.header.seq=0 34 | marker.id=0 35 | marker.header.stamp= rospy.Time() 36 | marker.header.frame_id= "vision_odometry_frame" #Must be a frame that exists (e.g. Spot's vision_odometry_frame) 37 | marker.type = visualization_msgs.msg.Marker.CUBE_LIST 38 | marker.action = visualization_msgs.msg.Marker.ADD 39 | marker.pose.position.x = 0 40 | marker.pose.position.y = 0 41 | marker.pose.position.z = 0 42 | marker.pose.orientation.x = 0.0 43 | marker.pose.orientation.y = 0.0 44 | marker.pose.orientation.z = 0.0 45 | marker.pose.orientation.w = 1.0 46 | marker.scale.x = cell_size 47 | marker.scale.y = cell_size 48 | marker.scale.z = cell_size 49 | marker.color.r = 0.5 50 | marker.color.g = 0.5 51 | marker.color.b = 0.5 52 | marker.color.a = 1 53 | 54 | for terrain_pt in terrain_pts: 55 | p = geometry_msgs.msg.Point() 56 | c = std_msgs.msg.ColorRGBA() 57 | 58 | # Cube location 59 | p.x = terrain_pt[0] 60 | p.y = terrain_pt[1] 61 | p.z = terrain_pt[2] 62 | 63 | #Make color change with distance from Spot 64 | # max_dist = math.sqrt(local_grid_proto[0].local_grid.extent.num_cells_x**2 + local_grid_proto[0].local_grid.extent.num_cells_y**2)/2.0 * local_grid_proto[0].local_grid.extent.cell_size 65 | # c.r = math.sqrt(p.x**2+p.y**2)/(max_dist+0.1) 66 | # c.g = 1 - math.sqrt(p.x**2+p.y**2)/(max_dist) 67 | # c.b = math.sqrt(p.x**2+p.y**2)/(max_dist+0.1) 68 | # c.a = 1.0 69 | 70 | marker.points.append(p) 71 | # marker.colors.append(c) 72 | 73 | return marker 74 | 75 | def get_terrain_grid(local_grid_proto): 76 | """Generate a 3xN set of points representing the terrain local grid.""" 77 | cells_pz_full = unpack_grid(local_grid_proto).astype(np.float32) 78 | # Populate the x,y values with a complete combination of all possible pairs for the dimensions in the grid extent. 79 | ys, xs = np.mgrid[0:local_grid_proto.local_grid.extent.num_cells_x, 0:local_grid_proto. 80 | local_grid.extent.num_cells_y] 81 | # Numpy vstack makes it so that each column is (x,y,z) for a single terrain point. The height values (z) come from the 82 | # terrain grid's data field. 83 | pts = np.vstack( 84 | [np.ravel(xs).astype(np.float32), 85 | np.ravel(ys).astype(np.float32), cells_pz_full]).T 86 | pts[:, [0, 1]] *= (local_grid_proto.local_grid.extent.cell_size, 87 | local_grid_proto.local_grid.extent.cell_size) 88 | return pts 89 | 90 | def unpack_grid(local_grid_proto): 91 | """Unpack the local grid proto.""" 92 | # Determine the data type for the bytes data. 93 | data_type = get_numpy_data_type(local_grid_proto.local_grid) 94 | if data_type is None: 95 | print("Cannot determine the dataformat for the local grid.") 96 | return None 97 | # Decode the local grid. 98 | if local_grid_proto.local_grid.encoding == local_grid_pb2.LocalGrid.ENCODING_RAW: 99 | full_grid = np.fromstring(local_grid_proto.local_grid.data, dtype=data_type) 100 | elif local_grid_proto.local_grid.encoding == local_grid_pb2.LocalGrid.ENCODING_RLE: 101 | full_grid = expand_data_by_rle_count(local_grid_proto, data_type=data_type) 102 | else: 103 | # Return nothing if there is no encoding type set. 104 | return None 105 | # Apply the offset and scaling to the local grid. 106 | if local_grid_proto.local_grid.cell_value_scale == 0: 107 | return full_grid 108 | full_grid_float = full_grid.astype(np.float64) 109 | full_grid_float *= local_grid_proto.local_grid.cell_value_scale 110 | full_grid_float += local_grid_proto.local_grid.cell_value_offset 111 | return full_grid_float 112 | 113 | def get_numpy_data_type(local_grid_proto): 114 | """Convert the cell format of the local grid proto to a numpy data type.""" 115 | if local_grid_proto.cell_format == local_grid_pb2.LocalGrid.CELL_FORMAT_UINT16: 116 | return np.uint16 117 | elif local_grid_proto.cell_format == local_grid_pb2.LocalGrid.CELL_FORMAT_INT16: 118 | return np.int16 119 | elif local_grid_proto.cell_format == local_grid_pb2.LocalGrid.CELL_FORMAT_UINT8: 120 | return np.uint8 121 | elif local_grid_proto.cell_format == local_grid_pb2.LocalGrid.CELL_FORMAT_INT8: 122 | return np.int8 123 | elif local_grid_proto.cell_format == local_grid_pb2.LocalGrid.CELL_FORMAT_FLOAT64: 124 | return np.float64 125 | elif local_grid_proto.cell_format == local_grid_pb2.LocalGrid.CELL_FORMAT_FLOAT32: 126 | return np.float32 127 | else: 128 | return None 129 | 130 | def expand_data_by_rle_count(local_grid_proto, data_type=np.int16): 131 | """Expand local grid data to full bytes data using the RLE count.""" 132 | cells_pz = np.fromstring(local_grid_proto.local_grid.data, dtype=data_type) 133 | cells_pz_full = [] 134 | # For each value of rle_counts, we expand the cell data at the matching index 135 | # to have that many repeated, consecutive values. 136 | for i in range(0, len(local_grid_proto.local_grid.rle_counts)): 137 | for j in range(0, local_grid_proto.local_grid.rle_counts[i]): 138 | cells_pz_full.append(cells_pz[i]) 139 | return np.array(cells_pz_full) 140 | 141 | def offset_grid_pixels(pts, vision_tform_local_grid, cell_size): 142 | """Offset the local grid's pixels to be in the world frame instead of the local grid frame.""" 143 | x_base = vision_tform_local_grid.position.x + cell_size * 0.5 144 | y_base = vision_tform_local_grid.position.y + cell_size * 0.5 145 | pts[:, 0] += x_base 146 | pts[:, 1] += y_base 147 | return pts 148 | 149 | -------------------------------------------------------------------------------- /spot_urdf/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(spot_urdf) 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) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a exec_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if your package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES spot_urdf 104 | # CATKIN_DEPENDS other_catkin_pkg 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | include_directories( 115 | # include 116 | # ${catkin_INCLUDE_DIRS} 117 | ) 118 | 119 | ## Declare a C++ library 120 | # add_library(${PROJECT_NAME} 121 | # src/${PROJECT_NAME}/spot_urdf.cpp 122 | # ) 123 | 124 | ## Add cmake target dependencies of the library 125 | ## as an example, code may need to be generated before libraries 126 | ## either from message generation or dynamic reconfigure 127 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 128 | 129 | ## Declare a C++ executable 130 | ## With catkin_make all packages are built within a single CMake context 131 | ## The recommended prefix ensures that target names across packages don't collide 132 | # add_executable(${PROJECT_NAME}_node src/spot_urdf_node.cpp) 133 | 134 | ## Rename C++ executable without prefix 135 | ## The above recommended prefix causes long target names, the following renames the 136 | ## target back to the shorter version for ease of user use 137 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 138 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 139 | 140 | ## Add cmake target dependencies of the executable 141 | ## same as for the library above 142 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 143 | 144 | ## Specify libraries to link a library or executable target against 145 | # target_link_libraries(${PROJECT_NAME}_node 146 | # ${catkin_LIBRARIES} 147 | # ) 148 | 149 | ############# 150 | ## Install ## 151 | ############# 152 | 153 | # all install targets should use catkin DESTINATION variables 154 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 155 | 156 | ## Mark executable scripts (Python etc.) for installation 157 | ## in contrast to setup.py, you can choose the destination 158 | # install(PROGRAMS 159 | # scripts/my_python_script 160 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark executables for installation 164 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 165 | # install(TARGETS ${PROJECT_NAME}_node 166 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 167 | # ) 168 | 169 | ## Mark libraries for installation 170 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 171 | # install(TARGETS ${PROJECT_NAME} 172 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 175 | # ) 176 | 177 | ## Mark cpp header files for installation 178 | # install(DIRECTORY include/${PROJECT_NAME}/ 179 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 180 | # FILES_MATCHING PATTERN "*.h" 181 | # PATTERN ".svn" EXCLUDE 182 | # ) 183 | 184 | ## Mark other files for installation (e.g. launch and bag files, etc.) 185 | # install(FILES 186 | # # myfile1 187 | # # myfile2 188 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 189 | # ) 190 | 191 | ############# 192 | ## Testing ## 193 | ############# 194 | 195 | ## Add gtest based cpp test target and link libraries 196 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_spot_urdf.cpp) 197 | # if(TARGET ${PROJECT_NAME}-test) 198 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 199 | # endif() 200 | 201 | ## Add folders to be run by python nosetests 202 | # catkin_add_nosetests(test) 203 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 2 | # Introduction 3 | Enable programmatic control of the Boston Dynamic Spot robot through the [Robot Operating System](https://www.ros.org/) (ROS) interface. 4 | 5 | # Overview 6 | 7 | The [spot_ros_interface](./spot_ros_interface/) ROS package provides a ROS interface to the Spot API converting ROS messages to API/gRPC calls to the Spot robot. 8 | 9 | ## Architecture Overview 10 | ![System Architecture](./docs/SystemArchitecture.png) 11 | 12 | 13 | # Getting Started 14 | ## 1. Installation process 15 | 16 | #### 1.1 Set up WSL (Ubuntu 20.04): 17 | https://docs.microsoft.com/en-us/windows/wsl/install-win10 18 | 19 | #### 1.2 Download and install Spot SDK (in WSL environment): 20 | https://github.com/boston-dynamics/spot-sdk/blob/master/docs/python/quickstart.md#getting-the-code 21 | 22 | #### 1.3 Upgrade Spot and Spot SDK to version 2.01 23 | 24 | Follow [Boston Dynamics' instructions for upgrading](https://www.bostondynamics.com/sites/default/files/inline-files/spot-2.0.1-release-notes.pdf) 25 | 26 | #### 1.4 Set up ROS Noetic in WSL: 27 | - Configure your Ubuntu repositories to allow "restricted," "universe," and "multiverse". Follow instructions [here](https://help.ubuntu.com/community/Repositories/CommandLine). 28 | Remember to `sudo apt-get update` after this. 29 | 30 | 31 | - Set up sources.list 32 | ``` 33 | sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' 34 | ``` 35 | - Set up keys 36 | ``` 37 | sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 38 | ``` 39 | If you experience issues connecting to the keyserver, you can try substituting `hkp://pgp.mit.edu:80` 40 | 41 | Alternatively, you can use curl instead of the apt-key command, which can be helpful if you are behind a proxy server: 42 | 43 | ``` 44 | curl -sSL 'http://keyserver.ubuntu.com/pks/lookup?op=get&search=0xC1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' | sudo apt-key add - 45 | ``` 46 | 47 | If you still experience issues like: 48 | `gpg: keyserver receive failed: No dirmngr` 49 | or 50 | `gpg: can't connect to the agent: IPC connect call failed` (see [here](https://github.com/microsoft/WSL/issues/5125) and [here](https://stackoverflow.com/questions/46673717/gpg-cant-connect-to-the-agent-ipc-connect-call-failed)), ensure `dirmngr` is installed: 51 | ``` 52 | sudo apt-get install dirmngr 53 | ``` 54 | If this still does not solve the problem: 55 | ``` 56 | sudo apt remove gpg 57 | sudo apt install gnupg1 58 | ``` 59 | You should now be able to re-run the key setup command above. 60 | - Install 61 | ``` 62 | sudo apt update 63 | sudo apt install ros-noetic-desktop-full 64 | ``` 65 | This will take some time. Go grab a coffee :coffee:. 66 | 67 | Once that is done, let us verify the installation. You must source this script in every bash terminal you use ROS in: 68 | ``` 69 | source /opt/ros/noetic/setup.bash 70 | ``` 71 | To automatically source it whenever you open a new terminal, add it to your .bashrc: 72 | ``` 73 | echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc 74 | source ~/.bashrc 75 | ``` 76 | *Note:* If you were in a virtual environment, you will have to re-activate your virtual environment after sourcing your .bashrc 77 | 78 | 79 | To verify the installation, run `roscore` in the terminal. This should start a roslaunch server without any errors. Feel free to kill the server (Ctrl+C) now. 80 | 81 | Congratulations! You have just set up ROS. Now let's install the the dependencies. 82 | 83 | 84 | #### 1.5 Install VcXSrv and Set up WSL Display 85 | 86 | Install VcXsrv: https://sourceforge.net/projects/vcxsrv/ 87 | 88 | Add the following to your .bashrc: 89 | ``` 90 | # Forward GUI windows to vcxsrv 91 | export DISPLAY=:0 92 | export LIBGL_ALWAYS_INDIRECT= 93 | ``` 94 | 95 | Start VcXsrv *without* native OpenGL: 96 | ![VcXsrv without native OpenGL](./docs/StartingVcXsrv.png) 97 | 98 | #### 1.6 Set Up URDF Visualization (Optional) 99 | 100 | Request spot.urdf and .stl mesh files from Boston Dynamics in order to be able to utilize the real-time RViz visualization of Spot's state. 101 | The directory structure must be as follows for compatibility with the `rviz_display.launch` file: 102 | 103 | ``` 104 | workspace_directory/ 105 | ... 106 | spot_urdf/ 107 | ... 108 | link_models/ 109 | base.stl 110 | fl.lleg.stl 111 | fr.hip.stl 112 | ... 113 | hl.uleg.stl 114 | hr.lleg.stl 115 | urdf/ 116 | spot.urdf 117 | ``` 118 | 119 | ## 2. Software dependencies 120 | 121 | Make `install_dependencies.sh` executable and run it: 122 | ``` 123 | cd ~/spot-ros-wrapper/src/install_script/ 124 | chmod +x install_dependencies.sh 125 | ./install_dependencies.sh 126 | ``` 127 | ## 3. Set up catkin workspace 128 | Now, go to your catkin workspace directory (e.g. `~/spot-ros-wrapper`) and setup your catkin workspace: 129 | ``` 130 | cd ~/spot-ros-wrapper 131 | catkin config --init --merge-devel --cmake-args -DCMAKE_BUILD_TYPE=Release 132 | catkin config --extend /opt/ros/noetic 133 | ``` 134 | 135 | *Note:* The structure of a catkin workspace must be as follows. For more info on catkin workspaces and packages, consult the [tutorials](http://wiki.ros.org/catkin/workspaces). 136 | ``` 137 | workspace_directory/ 138 | src/ 139 | package_1/ 140 | CMakeLists.txt 141 | package.xml 142 | scripts/ 143 | src/ 144 | ... 145 | package_n/ 146 | CMakeLists.txt 147 | package.xml 148 | scripts/ 149 | src/ 150 | build/ -- Auto-generated by catkin build 151 | devel/ -- Auto-generated by catkin build 152 | ``` 153 | 154 | # Build and Run 155 | Go to your catkin workspace directory, and build 156 | ``` 157 | cd ~/spot-ros-wrapper 158 | catkin build 159 | ``` 160 | 161 | After the build finishes, you must source the environment. **This must be done every time after building.** 162 | ``` 163 | source ~/spot-ros-wrapper/devel/setup.bash 164 | ``` 165 | 166 | Now let us start the ROS Wrapper Node. We first need to run `roscore`: 167 | ``` 168 | roscore 169 | ``` 170 | That process must be up and running whenever you want to run ROS Nodes. 171 | 172 | Open a new terminal, source your virtual environment and source the latest build: 173 | ``` 174 | # In a new terminal 175 | activate_venv spot_venv 176 | cd ~/spot-ros-wrapper 177 | source devel/setup.bash 178 | ``` 179 | And finally, let us start the ROS Wrapper node: 180 | ``` 181 | rosrun spot_ros_interface spot_ros_interface.py --username USERNAME --password PASSWORD 192.168.80.3 182 | ``` 183 | *Note:* Spot's default IP address is 192.168.80.3 over WiFi, and 10.0.0.3 over ethernet. 184 | 185 | *Note:* You must be able to ping Spot's IP addess in order to communicate. The easiest way to do so is to connect to Spot's Wi-Fi hotspot directly, or to its ethernet port. For more information, reference Spot's instruction manuals on ways to communicate. 186 | 187 | ### Controlling Spot from your keyboard 188 | 189 | With the ROS wrapper running, open a new terminal and run `keyboard_teleop.py` in the `spot_ros_interface` ROS package: 190 | ``` 191 | rosrun spot_ros_interface keyboard_teleop.py 192 | ``` 193 | and follow the instructions on screen. 194 | 195 | Ensure `spot_ros_interface.py` is running. 196 | 197 | *Note:* If Spot is in a faulty state and/or upside down, make sure to call the self-right command first (from the keyboard_teleop application, press "r"). 198 | 199 | ### To run occupancy grid visualizer 200 | 201 | ``` 202 | roslaunch spot_urdf rviz_display.launch 203 | ``` 204 | 205 | Potential Issues: 206 | 207 | - Spot model RViz visualization is white and not oriented properly 208 | - Solution: Ensure spot_ros_interface and robot_state_publisher nodes are running (the latter should have started by the rvis_display.launch file). 209 | 210 | - The occupancy grid is not being displayed: 211 | - Solution: Ensure that RViz is set to display a Marker type msg (on the left-hand panel), and that it is subscribed to the `occupancy_grid` topic. 212 | 213 | # ROS Package guidelines 214 | [ROS package guidelines](https://github.com/ethz-asl/mav_tools_public/wiki/How-to-Write-a-ROS-Package) 215 | 216 | Contribute 217 | 218 | This project has adopted the [Microsoft Open Source Code of Conduct](https://opensource.microsoft.com/codeofconduct/). For more information see the [Code of Conduct FAQ](https://opensource.microsoft.com/codeofconduct/faq/) or contact [opencode@microsoft.com](mailto:opencode@microsoft.com) with any additional questions or comments. 219 | 220 | 221 | -------------------------------------------------------------------------------- /spot_urdf/rviz/occupancy_grid_config.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /RobotModel1 10 | - /TF1 11 | - /Marker1 12 | Splitter Ratio: 0.5 13 | Tree Height: 1080 14 | - Class: rviz/Selection 15 | Name: Selection 16 | - Class: rviz/Tool Properties 17 | Expanded: 18 | - /2D Pose Estimate1 19 | - /2D Nav Goal1 20 | - /Publish Point1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.5886790156364441 23 | - Class: rviz/Views 24 | Expanded: 25 | - /Current View1 26 | - /Current View1/Focal Point1 27 | Name: Views 28 | Splitter Ratio: 0.5 29 | - Class: rviz/Time 30 | Experimental: false 31 | Name: Time 32 | SyncMode: 0 33 | SyncSource: "" 34 | Preferences: 35 | PromptSaveOnExit: true 36 | Toolbars: 37 | toolButtonStyle: 2 38 | Visualization Manager: 39 | Class: "" 40 | Displays: 41 | - Alpha: 0.5 42 | Cell Size: 1 43 | Class: rviz/Grid 44 | Color: 160; 160; 164 45 | Enabled: true 46 | Line Style: 47 | Line Width: 0.029999999329447746 48 | Value: Lines 49 | Name: Grid 50 | Normal Cell Count: 0 51 | Offset: 52 | X: 0 53 | Y: 0 54 | Z: 0 55 | Plane: XY 56 | Plane Cell Count: 10 57 | Reference Frame: 58 | Value: true 59 | - Alpha: 0.75 60 | Class: rviz/RobotModel 61 | Collision Enabled: false 62 | Enabled: true 63 | Links: 64 | All Links Enabled: true 65 | Expand Joint Details: false 66 | Expand Link Details: false 67 | Expand Tree: false 68 | Link Tree Style: Links in Alphabetic Order 69 | base_link: 70 | Alpha: 1 71 | Show Axes: false 72 | Show Trail: false 73 | Value: true 74 | fl.hip: 75 | Alpha: 1 76 | Show Axes: false 77 | Show Trail: false 78 | Value: true 79 | fl.lleg: 80 | Alpha: 1 81 | Show Axes: false 82 | Show Trail: false 83 | Value: true 84 | fl.uleg: 85 | Alpha: 1 86 | Show Axes: false 87 | Show Trail: false 88 | Value: true 89 | fr.hip: 90 | Alpha: 1 91 | Show Axes: false 92 | Show Trail: false 93 | Value: true 94 | fr.lleg: 95 | Alpha: 1 96 | Show Axes: false 97 | Show Trail: false 98 | Value: true 99 | fr.uleg: 100 | Alpha: 1 101 | Show Axes: false 102 | Show Trail: false 103 | Value: true 104 | hl.hip: 105 | Alpha: 1 106 | Show Axes: false 107 | Show Trail: false 108 | Value: true 109 | hl.lleg: 110 | Alpha: 1 111 | Show Axes: false 112 | Show Trail: false 113 | Value: true 114 | hl.uleg: 115 | Alpha: 1 116 | Show Axes: false 117 | Show Trail: false 118 | Value: true 119 | hr.hip: 120 | Alpha: 1 121 | Show Axes: false 122 | Show Trail: false 123 | Value: true 124 | hr.lleg: 125 | Alpha: 1 126 | Show Axes: false 127 | Show Trail: false 128 | Value: true 129 | hr.uleg: 130 | Alpha: 1 131 | Show Axes: false 132 | Show Trail: false 133 | Value: true 134 | Name: RobotModel 135 | Robot Description: robot_description 136 | TF Prefix: "" 137 | Update Interval: 0 138 | Value: true 139 | Visual Enabled: true 140 | - Class: rviz/TF 141 | Enabled: true 142 | Frame Timeout: 15 143 | Frames: 144 | All Enabled: true 145 | base_link: 146 | Value: true 147 | fl.hip: 148 | Value: true 149 | fl.lleg: 150 | Value: true 151 | fl.uleg: 152 | Value: true 153 | fr.hip: 154 | Value: true 155 | fr.lleg: 156 | Value: true 157 | fr.uleg: 158 | Value: true 159 | hl.hip: 160 | Value: true 161 | hl.lleg: 162 | Value: true 163 | hl.uleg: 164 | Value: true 165 | hr.hip: 166 | Value: true 167 | hr.lleg: 168 | Value: true 169 | hr.uleg: 170 | Value: true 171 | vision_odometry_frame: 172 | Value: true 173 | Marker Scale: 0.5 174 | Name: TF 175 | Show Arrows: true 176 | Show Axes: true 177 | Show Names: true 178 | Tree: 179 | vision_odometry_frame: 180 | base_link: 181 | fl.hip: 182 | fl.uleg: 183 | fl.lleg: 184 | {} 185 | fr.hip: 186 | fr.uleg: 187 | fr.lleg: 188 | {} 189 | hl.hip: 190 | hl.uleg: 191 | hl.lleg: 192 | {} 193 | hr.hip: 194 | hr.uleg: 195 | hr.lleg: 196 | {} 197 | Update Interval: 0 198 | Value: true 199 | - Class: rviz/Marker 200 | Enabled: true 201 | Marker Topic: occupancy_grid 202 | Name: Marker 203 | Namespaces: 204 | "": true 205 | Queue Size: 20 206 | Value: true 207 | Enabled: true 208 | Global Options: 209 | Background Color: 48; 48; 48 210 | Default Light: true 211 | Fixed Frame: vision_odometry_frame 212 | Frame Rate: 30 213 | Name: root 214 | Tools: 215 | - Class: rviz/Interact 216 | Hide Inactive Objects: true 217 | - Class: rviz/MoveCamera 218 | - Class: rviz/Select 219 | - Class: rviz/FocusCamera 220 | - Class: rviz/Measure 221 | - Class: rviz/SetInitialPose 222 | Theta std deviation: 0.2617993950843811 223 | Topic: /initialpose 224 | X std deviation: 0.5 225 | Y std deviation: 0.5 226 | - Class: rviz/SetGoal 227 | Topic: /move_base_simple/goal 228 | - Class: rviz/PublishPoint 229 | Single click: true 230 | Topic: /clicked_point 231 | Value: true 232 | Views: 233 | Current: 234 | Class: rviz/Orbit 235 | Distance: 5 236 | Enable Stereo Rendering: 237 | Stereo Eye Separation: 0.05999999865889549 238 | Stereo Focal Distance: 1 239 | Swap Stereo Eyes: false 240 | Value: false 241 | Focal Point: 242 | X: 0 243 | Y: 0 244 | Z: 0 245 | Focal Shape Fixed Size: false 246 | Focal Shape Size: 0.05000000074505806 247 | Invert Z Axis: false 248 | Name: Current View 249 | Near Clip Distance: 0.009999999776482582 250 | Pitch: 0.6000000238418579 251 | Target Frame: base_link 252 | Value: Orbit (rviz) 253 | Yaw: -0.5 254 | Saved: ~ 255 | Window Geometry: 256 | Displays: 257 | collapsed: false 258 | Height: 1377 259 | Hide Left Dock: false 260 | Hide Right Dock: false 261 | QMainWindow State: 000000ff00000000fd000000040000000000000156000004c1fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000004c1000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c1fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000004c1000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a0000000044fc0100000002fb0000000800540069006d0065010000000000000a000000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000078f000004c100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 262 | Selection: 263 | collapsed: false 264 | Time: 265 | collapsed: false 266 | Tool Properties: 267 | collapsed: false 268 | Views: 269 | collapsed: false 270 | Width: 2560 271 | X: 0 272 | Y: 23 273 | -------------------------------------------------------------------------------- /spot_ros_interface/scripts/spot_ros_interface.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import argparse 4 | import logging 5 | import math 6 | import sys 7 | import os 8 | import subprocess 9 | import time 10 | import numpy as np 11 | 12 | # Bosdyn specific imports 13 | import bosdyn.client 14 | import bosdyn.client.estop 15 | import bosdyn.client.lease 16 | import bosdyn.client.util 17 | import bosdyn.geometry 18 | 19 | from bosdyn.client import math_helpers 20 | from bosdyn.client.image import ImageClient 21 | from bosdyn.client.robot_command import RobotCommandBuilder, RobotCommandClient 22 | from bosdyn.client.robot_state import RobotStateClient 23 | from bosdyn.client.local_grid import LocalGridClient 24 | from bosdyn.api import trajectory_pb2, image_pb2, robot_state_pb2, local_grid_pb2 25 | 26 | from bosdyn.client.frame_helpers import get_a_tform_b, get_vision_tform_body, get_odom_tform_body,\ 27 | BODY_FRAME_NAME, GRAV_ALIGNED_BODY_FRAME_NAME, VISION_FRAME_NAME, ODOM_FRAME_NAME 28 | 29 | # ROS specific imports 30 | import rospy 31 | import diagnostic_msgs.msg 32 | import geometry_msgs.msg 33 | import std_msgs.msg 34 | import sensor_msgs.msg 35 | import visualization_msgs.msg 36 | import spot_ros_msgs.msg 37 | import spot_ros_srvs.srv 38 | 39 | import tf2_ros 40 | 41 | from grid_utils import get_terrain_markers 42 | 43 | class SpotInterface: 44 | '''Callbacks for an instance of a Spot robot''' 45 | 46 | # 0.6 s is the standard duration for cmds in boston dynamics Spot examples 47 | VELOCITY_CMD_DURATION = 0.6 # [seconds] 48 | TRAJECTORY_CMD_TIMEOUT = 20.0 # [seconds] 49 | x_goal_tolerance = 0.05 #[m] 50 | y_goal_tolerance = 0.05 #[m] 51 | angle_goal_tolerance = 0.075 #[rad] 52 | LOGGER = logging.getLogger() 53 | 54 | def __init__(self, config): 55 | # Ensure interface can ping Spot 56 | try: 57 | with open(os.devnull, 'wb') as devnull: 58 | resp = subprocess.check_call(['ping', '-c', '1', config.hostname], stdout=devnull, stderr=subprocess.STDOUT) 59 | if resp != 0: 60 | print ("ERROR: Cannot detect a Spot with IP: {}.\nMake sure Spot is powered on and on the same network".format(config.hostname)) 61 | sys.exit() 62 | except: 63 | print("ERROR: Cannot detect a Spot with IP: {}.\nMake sure Spot is powered on and on the same network".format(config.hostname)) 64 | sys.exit() 65 | 66 | # Set up SDK 67 | bosdyn.client.util.setup_logging(config.verbose) 68 | self.sdk = bosdyn.client.create_standard_sdk('spot_ros_interface_sdk') 69 | self.sdk.load_app_token(config.app_token) 70 | 71 | # Create instance of a robot 72 | try: 73 | self.robot = self.sdk.create_robot(config.hostname) 74 | self.robot.authenticate(config.username, config.password) 75 | self.robot.time_sync.wait_for_sync() 76 | except bosdyn.client.RpcError as err: 77 | self.LOGGER.error("Failed to communicate with robot: %s", err) 78 | 79 | # Client to send cmds to Spot 80 | self.command_client = self.robot.ensure_client( 81 | RobotCommandClient.default_service_name) 82 | 83 | # Client to request images from Spot 84 | self.image_client = self.robot.ensure_client( 85 | ImageClient.default_service_name) 86 | 87 | self.image_source_names = [ 88 | src.name for src in self.image_client.list_image_sources() if "image" in src.name 89 | ] 90 | 91 | self.depth_image_sources = [ 92 | src.name for src in self.image_client.list_image_sources() if "depth" in src.name 93 | ] 94 | 95 | # Client to request robot state 96 | self.robot_state_client = self.robot.ensure_client( 97 | RobotStateClient.default_service_name) 98 | 99 | # Client to request local occupancy grid 100 | self.grid_client = self.robot.ensure_client( 101 | LocalGridClient.default_service_name) 102 | self.local_grid_types = self.grid_client.get_local_grid_types() 103 | 104 | # Spot requires a software estop to be activated. 105 | estop_client = self.robot.ensure_client( 106 | bosdyn.client.estop.EstopClient.default_service_name) 107 | self.estop_endpoint = bosdyn.client.estop.EstopEndpoint( 108 | client=estop_client, name='spot_ros_interface', estop_timeout=9.0) 109 | self.estop_endpoint.force_simple_setup() 110 | 111 | # Only one client at a time can operate a robot. 112 | self.lease_client = self.robot.ensure_client( 113 | bosdyn.client.lease.LeaseClient.default_service_name) 114 | try: 115 | self.lease = self.lease_client.acquire() 116 | except bosdyn.client.lease.ResourceAlreadyClaimedError as err: 117 | print("ERROR: Lease cannot be acquired. Ensure no other client has the lease. Shutting down.") 118 | print(err) 119 | sys.exit() 120 | 121 | # True for RViz visualization of Spot in 3rd person with occupancy grid 122 | self.third_person_view = True 123 | 124 | # Power on motors 125 | self.motors_on = config.motors_on.lower()!="n" 126 | 127 | ### Callback functions ### 128 | 129 | def self_right_cmd_srv(self, stand): 130 | """ Callback that sends self-right cmd""" 131 | cmd = RobotCommandBuilder.selfright_command() 132 | ret = self.command_client.robot_command(cmd) 133 | rospy.loginfo("Robot self right cmd sent. {}".format(ret)) 134 | 135 | return [] 136 | 137 | def stand_cmd_srv(self, stand): 138 | """Callback that sends stand cmd at a given height delta [m] from standard configuration""" 139 | 140 | cmd = RobotCommandBuilder.stand_command( 141 | body_height=stand.body_pose.translation.z, 142 | footprint_R_body=self.quat_to_euler(stand.body_pose.rotation) 143 | ) 144 | ret = self.command_client.robot_command(cmd) 145 | rospy.loginfo("Robot stand cmd sent. {}".format(ret)) 146 | 147 | return [] 148 | 149 | def trajectory_cmd_srv(self, trajectory): 150 | ''' 151 | Callback that specifies waypoint(s) (Point) [m] with a final orientation [rad] 152 | 153 | The name of the frame that trajectory is relative to. 154 | The trajectory must be expressed in a gravity aligned frame, so either "vision", "odom", or "flat_body". 155 | Any other provided se2_frame_name will be rejected and the trajectory command will not be exectuted. 156 | ''' 157 | # TODO: Support other reference frames (currently only VISION ref. frame) 158 | 159 | for pose in trajectory.waypoints.poses: 160 | x = pose.position.x 161 | y = pose.position.y 162 | heading = math.atan2(y,x) 163 | frame = VISION_FRAME_NAME 164 | 165 | cmd = RobotCommandBuilder.trajectory_command( 166 | goal_x=x, 167 | goal_y=y, 168 | goal_heading=heading, 169 | frame_name=frame, 170 | ) 171 | self.command_client.robot_command(lease=None, command=cmd, end_time_secs=time.time() + self.TRAJECTORY_CMD_TIMEOUT) 172 | 173 | robot_state = self.get_robot_state()[0].vision_tform_body 174 | final_pose = geometry_msgs.msg.Pose() 175 | final_pose.position = robot_state.translation 176 | final_pose.orientation = robot_state.rotation 177 | 178 | spot_ros_srvs.srv.TrajectoryResponse(final_pose) 179 | 180 | def velocity_cmd_srv(self, twist): 181 | """Callback that sends instantaneous velocity [m/s] commands to Spot""" 182 | 183 | v_x = twist.velocity.linear.x 184 | v_y = twist.velocity.linear.y 185 | v_rot = twist.velocity.angular.z 186 | 187 | cmd = RobotCommandBuilder.velocity_command( 188 | v_x=v_x, 189 | v_y=v_y, 190 | v_rot=v_rot 191 | ) 192 | 193 | self.command_client.robot_command( 194 | cmd, 195 | end_time_secs=time.time() + self.VELOCITY_CMD_DURATION 196 | ) 197 | rospy.loginfo( 198 | "Robot velocity cmd sent: v_x=${},v_y=${},v_rot${}".format(v_x, v_y, v_rot)) 199 | return [] 200 | 201 | ### Helper functions ### 202 | 203 | def block_until_pose_reached(self, cmd, goal): 204 | """Do not return until goal waypoint is reached, or TRAJECTORY_CMD_TIMEOUT is reached.""" 205 | # TODO: Make trajectory_cmd_timeout part of the service request 206 | 207 | self.command_client.robot_command( 208 | cmd, 209 | end_time_secs = time.time()+self.TRAJECTORY_CMD_TIMEOUT if self.TRAJECTORY_CMD_TIMEOUT else None 210 | ) 211 | 212 | start_time = time.time() 213 | current_time = time.time() 214 | while (not self.is_final_state(goal) and (current_time - start_time < self.TRAJECTORY_CMD_TIMEOUT if self.TRAJECTORY_CMD_TIMEOUT else True)): 215 | time.sleep(.25) 216 | current_time = time.time() 217 | return self.is_final_state(goal) 218 | 219 | def is_final_state(self, goal): 220 | """Check if the current robot state is within range of the specified position.""" 221 | goal_x=goal[0] 222 | goal_y=goal[1] 223 | goal_heading=goal[2] 224 | robot_state = self.get_robot_state()[0].vision_tform_body 225 | robot_pose = robot_state.translation 226 | robot_angle = self.quat_to_euler((robot_state.rotation.x, robot_state.rotation.y, 227 | robot_state.rotation.z, robot_state.rotation.w)).yaw 228 | 229 | x_dist = abs(goal_x - robot_pose.x) 230 | y_dist = abs(goal_y - robot_pose.y) 231 | angle = abs(goal_heading - robot_angle) 232 | if ((x_dist < self.x_goal_tolerance) and (y_dist < self.y_goal_tolerance) and (angle < self.angle_goal_tolerance)): 233 | return True 234 | return False 235 | 236 | def quat_to_euler(self, quat): 237 | """Convert a quaternion to xyz Euler angles.""" 238 | q = [quat.x, quat.y, quat.z, quat.w] 239 | roll = math.atan2(2 * q[3] * q[0] + q[1] * q[2], 1 - 2 * q[0]**2 + 2 * q[1]**2) 240 | pitch = math.atan2(2 * q[1] * q[3] - 2 * q[0] * q[2], 1 - 2 * q[1]**2 - 2 * q[2]**2) 241 | yaw = math.atan2(2 * q[2] * q[3] + 2 * q[0] * q[1], 1 - 2 * q[1]**2 - 2 * q[2]**2) 242 | return bosdyn.geometry.EulerZXY(yaw=yaw, roll=roll, pitch=pitch) 243 | 244 | # TODO: Unit test the get_state method conversion from pbuf to ROS msg (test repeated fields, etc) 245 | def get_robot_state(self): 246 | ''' Returns tuple of kinematic_state, robot_state 247 | kinematic_state: 248 | timestamp 249 | joint_states [] 250 | ko_tform_body 251 | body_twist_rt_ko 252 | ground_plane_rt_ko 253 | vo_tform_body 254 | robot_state: 255 | power_state 256 | battery_states[] 257 | comms_states[] 258 | system_fault_state 259 | estop_states[] 260 | behavior_fault_state 261 | ''' 262 | robot_state = self.robot_state_client.get_robot_state() 263 | rs_msg = spot_ros_msgs.msg.RobotState() 264 | 265 | ''' PowerState conversion ''' 266 | rs_msg.power_state.header.stamp.secs = robot_state.power_state.timestamp.seconds 267 | rs_msg.power_state.header.stamp.nsecs = robot_state.power_state.timestamp.nanos 268 | rs_msg.power_state.motor_power_state = robot_state.power_state.motor_power_state #[enum] 269 | rs_msg.power_state.shore_power_state = robot_state.power_state.shore_power_state #[enum] 270 | rs_msg.power_state.locomotion_charge_percentage = robot_state.power_state.locomotion_charge_percentage.value #[google.protobuf.DoubleValue] 271 | rs_msg.power_state.locomotion_estimated_runtime.secs = robot_state.power_state.locomotion_estimated_runtime.seconds #[google.protobuf.Duration] 272 | 273 | ''' BatteryState conversion [repeated field] ''' 274 | for battery_state in robot_state.battery_states: 275 | battery_state_msg = sensor_msgs.msg.BatteryState() 276 | 277 | header = std_msgs.msg.Header() 278 | header.stamp.secs = battery_state.timestamp.seconds 279 | header.stamp.nsecs = battery_state.timestamp.nanos 280 | header.frame_id = battery_state.identifier #[string] 281 | 282 | battery_state_msg.header = header 283 | 284 | battery_state_msg.percentage = battery_state.charge_percentage.value/100 #[double] 285 | # NOTE: Using battery_state_msg.charge as the estimated runtime in sec 286 | battery_state_msg.charge = battery_state.estimated_runtime.seconds #[google.protobuf.Duration] 287 | battery_state_msg.current = battery_state.current.value #[DoubleValue] 288 | battery_state_msg.voltage = battery_state.voltage.value #[DoubleValue] 289 | # NOTE: Ignoring battery_state.temperatures for now; no field in BatteryState maps directly to it 290 | battery_state_msg.power_supply_status = battery_state.status #[enum] 291 | 292 | rs_msg.battery_states.append(battery_state_msg) 293 | 294 | ''' CommsState conversion [repeated field] ''' 295 | for comms_state in robot_state.comms_states: 296 | comms_state_msg = spot_ros_msgs.msg.CommsState() 297 | 298 | comms_state_msg.header.stamp.secs = comms_state.timestamp.seconds #[google.protobuf.Timestamp] 299 | comms_state_msg.header.stamp.nsecs = comms_state.timestamp.nanos #[google.protobuf.Timestamp] 300 | comms_state_msg.wifi_mode = comms_state.wifi_state.current_mode #[enum] Note: wifi_state is oneof 301 | comms_state_msg.essid = comms_state.wifi_state.essid #[string] 302 | 303 | rs_msg.comms_states.append(comms_state_msg) 304 | 305 | ''' SystemFaultState conversion ''' 306 | ### faults is Repeated ### 307 | for fault in robot_state.system_fault_state.faults: 308 | system_fault_msg = spot_ros_msgs.msg.SystemFault() 309 | 310 | system_fault_msg.header.frame_id = fault.name #[string] 311 | system_fault_msg.header.stamp.secs = fault.onset_timestamp.seconds #[google.protobuf.Timestamp] 312 | system_fault_msg.header.stamp.nsecs = fault.onset_timestamp.nanos #[google.protobuf.Timestamp] 313 | system_fault_msg.duration.secs = fault.duration.seconds #[google.protobuf.Duration] 314 | system_fault_msg.duration.nsecs = fault.duration.nanos #[google.protobuf.Duration] 315 | system_fault_msg.code = fault.code #[int32] 316 | system_fault_msg.uid = fault.uid #[uint64] 317 | system_fault_msg.error_message = fault.error_message #[string] 318 | system_fault_msg.attributes = fault.attributes #[repeated-string] 319 | system_fault_msg.severity = fault.severity #[enum] 320 | 321 | rs_msg.system_fault_state.faults.append(system_fault_msg) 322 | 323 | ### historical_faults is Repeated ### 324 | for historical_fault in robot_state.system_fault_state.faults: 325 | system_fault_msg = spot_ros_msgs.msg.SystemFault() 326 | 327 | system_fault_msg.header.frame_id = historical_fault.name #[string] 328 | system_fault_msg.header.stamp.secs = historical_fault.onset_timestamp.seconds #[google.protobuf.Timestamp] 329 | system_fault_msg.header.stamp.nsecs = historical_fault.onset_timestamp.nanos #[google.protobuf.Timestamp] 330 | system_fault_msg.duration.secs = historical_fault.duration.seconds #[google.protobuf.Duration] 331 | system_fault_msg.duration.nsecs = historical_fault.duration.nanos #[google.protobuf.Duration] 332 | system_fault_msg.code = historical_fault.code #[int32] 333 | system_fault_msg.uid = historical_fault.uid #[uint64] 334 | system_fault_msg.error_message = historical_fault.error_message #[string] 335 | system_fault_msg.attributes = historical_fault.attributes #[repeated-string] 336 | system_fault_msg.severity = historical_fault.severity #[enum] 337 | 338 | rs_msg.system_fault_state.historical_faults.append(system_fault_msg) 339 | 340 | #[map] 341 | if robot_state.system_fault_state.aggregated: 342 | for key, value in robot_state.system_fault_state.aggregated.items(): 343 | kv = diagnostic_msgs.msg.KeyValue() 344 | kv.key = key 345 | kv.value = value 346 | rs_msg.system_fault_state.aggregated.append(kv) 347 | 348 | ''' EStopState conversion [repeated field] ''' 349 | for estop_state in robot_state.estop_states: 350 | estop_msg = spot_ros_msgs.msg.EStopState() 351 | 352 | estop_msg.header.stamp.secs = estop_state.timestamp.seconds #[google.protobuf.Timestamp] 353 | estop_msg.header.stamp.nsecs = estop_state.timestamp.nanos #[google.protobuf.Timestamp] 354 | estop_msg.header.frame_id = estop_state.name #[string] 355 | estop_msg.type = estop_state.type #[enum] 356 | estop_msg.state = estop_state.state #[enum] 357 | estop_msg.state_description = estop_state.state_description #[string] 358 | 359 | rs_msg.estop_states.append(estop_msg) 360 | 361 | ''' KinematicState conversion ''' 362 | ks_msg = spot_ros_msgs.msg.KinematicState() 363 | 364 | ks_msg.header.stamp.secs = robot_state.kinematic_state.acquisition_timestamp.seconds 365 | ks_msg.header.stamp.nsecs = robot_state.kinematic_state.acquisition_timestamp.nanos 366 | 367 | ### joint_states is repeated ### 368 | js = sensor_msgs.msg.JointState() 369 | js.header.stamp = ks_msg.header.stamp 370 | for joint_state in robot_state.kinematic_state.joint_states: 371 | js.name.append(joint_state.name) # [string] 372 | js.position.append(joint_state.position.value) # Note: angle in rad 373 | js.velocity.append(joint_state.velocity.value) # Note: ang vel 374 | # NOTE: ang accel. JointState doesn't have accel. Ignoring joint_state.acceleration for now. 375 | js.effort.append(joint_state.load.value) # Note: Torque in N-m 376 | 377 | ks_msg.joint_states = js 378 | 379 | # SE3Pose representing transform of Spot's Body frame relative to the inertial Vision frame 380 | vision_tform_body = get_vision_tform_body(robot_state.kinematic_state.transforms_snapshot) 381 | 382 | ks_msg.vision_tform_body.translation.x = vision_tform_body.x 383 | ks_msg.vision_tform_body.translation.y = vision_tform_body.y 384 | ks_msg.vision_tform_body.translation.z = vision_tform_body.z 385 | 386 | ks_msg.vision_tform_body.rotation.x = vision_tform_body.rot.x 387 | ks_msg.vision_tform_body.rotation.y = vision_tform_body.rot.y 388 | ks_msg.vision_tform_body.rotation.z = vision_tform_body.rot.z 389 | ks_msg.vision_tform_body.rotation.w = vision_tform_body.rot.w 390 | 391 | # odom_tform_body: SE3Pose representing transform of Spot's Body frame relative to the odometry frame 392 | odom_tform_body = get_odom_tform_body(robot_state.kinematic_state.transforms_snapshot) 393 | 394 | ks_msg.odom_tform_body.translation.x = odom_tform_body.x 395 | ks_msg.odom_tform_body.translation.y = odom_tform_body.y 396 | ks_msg.odom_tform_body.translation.z = odom_tform_body.z 397 | 398 | ks_msg.odom_tform_body.rotation.x = odom_tform_body.rot.x 399 | ks_msg.odom_tform_body.rotation.y = odom_tform_body.rot.y 400 | ks_msg.odom_tform_body.rotation.z = odom_tform_body.rot.z 401 | ks_msg.odom_tform_body.rotation.w = odom_tform_body.rot.w 402 | 403 | ''' velocity_of_body_in_vision ''' 404 | ks_msg.velocity_of_body_in_vision.linear.x = robot_state.kinematic_state.velocity_of_body_in_vision.linear.x 405 | ks_msg.velocity_of_body_in_vision.linear.y = robot_state.kinematic_state.velocity_of_body_in_vision.linear.y 406 | ks_msg.velocity_of_body_in_vision.linear.z = robot_state.kinematic_state.velocity_of_body_in_vision.linear.z 407 | 408 | ks_msg.velocity_of_body_in_vision.angular.x = robot_state.kinematic_state.velocity_of_body_in_vision.angular.x 409 | ks_msg.velocity_of_body_in_vision.angular.y = robot_state.kinematic_state.velocity_of_body_in_vision.angular.y 410 | ks_msg.velocity_of_body_in_vision.angular.z = robot_state.kinematic_state.velocity_of_body_in_vision.angular.z 411 | 412 | ''' velocity_of_body_in_odom ''' 413 | 414 | ks_msg.velocity_of_body_in_odom.linear.x = robot_state.kinematic_state.velocity_of_body_in_odom.linear.x 415 | ks_msg.velocity_of_body_in_odom.linear.y = robot_state.kinematic_state.velocity_of_body_in_odom.linear.y 416 | ks_msg.velocity_of_body_in_odom.linear.z = robot_state.kinematic_state.velocity_of_body_in_odom.linear.z 417 | 418 | ks_msg.velocity_of_body_in_odom.angular.x = robot_state.kinematic_state.velocity_of_body_in_odom.angular.x 419 | ks_msg.velocity_of_body_in_odom.angular.y = robot_state.kinematic_state.velocity_of_body_in_odom.angular.y 420 | ks_msg.velocity_of_body_in_odom.angular.z = robot_state.kinematic_state.velocity_of_body_in_odom.angular.z 421 | 422 | 423 | ### BehaviorFaultState conversion 424 | '''faults is repeated''' 425 | for fault in robot_state.behavior_fault_state.faults: 426 | behaviour_fault_state_msg = spot_ros_msgs.msg.BehaviorFaultState() 427 | 428 | behaviour_fault_state_msg.header.frame_id = fault.behavior_fault_id #[uint32] 429 | behaviour_fault_state_msg.header.stamp.secs = fault.onset_timestamp.seconds #[google.protobuf.Timestamp] 430 | behaviour_fault_state_msg.header.stamp.nsecs = fault.onset_timestamp.nanos #[google.protobuf.Timestamp] 431 | behaviour_fault_state_msg.cause = fault.cause #[enum] 432 | behaviour_fault_state_msg.status = fault.status #[enum] 433 | 434 | rs_msg.behavior_fault_states.append(behaviour_fault_state_msg) 435 | 436 | ### FootState conversion [repeated] 437 | for foot_state in robot_state.foot_state: 438 | foot_state_msg = spot_ros_msgs.msg.FootState() 439 | 440 | foot_state_msg.foot_position_rt_body.x = foot_state.foot_position_rt_body.x #[double] 441 | foot_state_msg.foot_position_rt_body.y = foot_state.foot_position_rt_body.y #[double] 442 | foot_state_msg.foot_position_rt_body.z = foot_state.foot_position_rt_body.z #[double] 443 | foot_state_msg.contact = foot_state.contact #[enum] 444 | 445 | rs_msg.foot_states.append(foot_state_msg) 446 | 447 | 448 | return ks_msg, rs_msg #kinematic state message, robot state message 449 | 450 | 451 | def start_spot_ros_interface(self): 452 | 453 | # ROS Node initialization 454 | rospy.init_node('spot_ros_interface_py') 455 | rate = rospy.Rate(200) # Update at 200 Hz 456 | 457 | # Each service will handle a specific command to Spot instance 458 | rospy.Service("self_right_cmd", spot_ros_srvs.srv.Stand, self.self_right_cmd_srv) 459 | rospy.Service("stand_cmd", spot_ros_srvs.srv.Stand, self.stand_cmd_srv) 460 | rospy.Service("trajectory_cmd", spot_ros_srvs.srv.Trajectory, self.trajectory_cmd_srv) 461 | rospy.Service("velocity_cmd", spot_ros_srvs.srv.Velocity, self.velocity_cmd_srv) 462 | 463 | # Single image publisher will publish all images from all Spot cameras 464 | kinematic_state_pub = rospy.Publisher( 465 | "kinematic_state", spot_ros_msgs.msg.KinematicState, queue_size=20) 466 | robot_state_pub = rospy.Publisher( 467 | "robot_state", spot_ros_msgs.msg.RobotState, queue_size=20) 468 | occupancy_grid_pub = rospy.Publisher( 469 | "occupancy_grid", visualization_msgs.msg.Marker, queue_size=20) 470 | 471 | # Publish tf2 from visual odometry frame to Spot's base link 472 | spot_tf_broadcaster = tf2_ros.TransformBroadcaster() 473 | 474 | # Publish static tf2 from Spot's base link to front-left camera 475 | spot_tf_static_broadcaster = tf2_ros.StaticTransformBroadcaster() 476 | 477 | image_only_pub = rospy.Publisher( 478 | "spot_image", sensor_msgs.msg.Image, queue_size=20) 479 | 480 | camera_info_pub = rospy.Publisher( 481 | "spot_cam_info", sensor_msgs.msg.CameraInfo, queue_size=20) 482 | 483 | # TODO: Publish depth images 484 | # depth_image_pub = rospy.Publisher( 485 | # "depth_image", sensor_msgs.msg.Image, queue_size=20) 486 | 487 | # For RViz 3rd person POV visualization 488 | if self.third_person_view: 489 | joint_state_pub = rospy.Publisher( 490 | "joint_state_from_spot", sensor_msgs.msg.JointState, queue_size=20) 491 | 492 | try: 493 | with bosdyn.client.lease.LeaseKeepAlive(self.lease_client), bosdyn.client.estop.EstopKeepAlive( 494 | self.estop_endpoint): 495 | rospy.loginfo("Acquired lease") 496 | if self.motors_on: 497 | rospy.loginfo("Powering on robot... This may take a several seconds.") 498 | self.robot.power_on(timeout_sec=20) 499 | assert self.robot.is_powered_on(), "Robot power on failed." 500 | rospy.loginfo("Robot powered on.") 501 | else: 502 | rospy.loginfo("Not powering on robot, continuing") 503 | 504 | while not rospy.is_shutdown(): 505 | ''' Publish Robot State''' 506 | kinematic_state, robot_state = self.get_robot_state() 507 | 508 | kinematic_state_pub.publish(kinematic_state) 509 | robot_state_pub.publish(robot_state) 510 | 511 | # Publish tf2 from the fixed vision_odometry_frame to the Spot's base_link 512 | t = geometry_msgs.msg.TransformStamped() 513 | t.header.stamp = rospy.Time.now() 514 | t.header.frame_id = "vision_odometry_frame" 515 | t.child_frame_id = "base_link" 516 | t.transform.translation.x = kinematic_state.vision_tform_body.translation.x 517 | t.transform.translation.y = kinematic_state.vision_tform_body.translation.y 518 | t.transform.translation.z = kinematic_state.vision_tform_body.translation.z 519 | t.transform.rotation.x = kinematic_state.vision_tform_body.rotation.x 520 | t.transform.rotation.y = kinematic_state.vision_tform_body.rotation.y 521 | t.transform.rotation.z = kinematic_state.vision_tform_body.rotation.z 522 | t.transform.rotation.w = kinematic_state.vision_tform_body.rotation.w 523 | spot_tf_broadcaster.sendTransform(t) 524 | 525 | if self.third_person_view: 526 | joint_state_pub.publish(kinematic_state.joint_states) 527 | 528 | ''' Publish Images''' 529 | img_reqs = [image_pb2.ImageRequest(image_source_name=source, image_format=image_pb2.Image.FORMAT_RAW) for source in self.image_source_names[2:3]] 530 | image_list = self.image_client.get_image(img_reqs) 531 | 532 | for img in image_list: 533 | if img.status == image_pb2.ImageResponse.STATUS_OK: 534 | 535 | header = std_msgs.msg.Header() 536 | header.stamp = t.header.stamp 537 | header.frame_id = img.source.name 538 | 539 | if img.shot.image.pixel_format == image_pb2.Image.PIXEL_FORMAT_DEPTH_U16: 540 | dtype = np.uint16 541 | else: 542 | dtype = np.uint8 543 | 544 | if img.shot.image.format == image_pb2.Image.FORMAT_RAW: 545 | image = np.fromstring(img.shot.image.data, dtype=dtype) 546 | image = image.reshape(img.shot.image.rows, img.shot.image.cols) 547 | 548 | # Make Image component of ImageCapture 549 | i = sensor_msgs.msg.Image() 550 | i.header = header 551 | i.width = img.shot.image.cols 552 | i.height = img.shot.image.rows 553 | i.data = img.shot.image.data if img.shot.image.format != image_pb2.Image.FORMAT_RAW else image.tobytes() 554 | i.step = img.shot.image.cols 555 | i.encoding = 'mono8' 556 | 557 | # CameraInfo 558 | cam_info = sensor_msgs.msg.CameraInfo() 559 | cam_info.header = i.header 560 | cam_info.width = i.width 561 | cam_info.height = i.height 562 | cam_info.distortion_model = "plumb_bob" 563 | cam_info.D = [0.0,0.0,0.0,0.0] 564 | f = img.source.pinhole.intrinsics.focal_length 565 | c = img.source.pinhole.intrinsics.principal_point 566 | cam_info.K = \ 567 | [f.x, 0, c.x, \ 568 | 0, f.y, c.y, \ 569 | 0, 0, 1] 570 | 571 | # Transform from base_link to camera for current img 572 | body_tform_cam = get_a_tform_b(img.shot.transforms_snapshot, 573 | BODY_FRAME_NAME, 574 | img.shot.frame_name_image_sensor) 575 | 576 | # Generate camera to body Transform 577 | body_tform_cam_tf = geometry_msgs.msg.Transform() 578 | body_tform_cam_tf.translation.x = body_tform_cam.position.x 579 | body_tform_cam_tf.translation.y = body_tform_cam.position.y 580 | body_tform_cam_tf.translation.z = body_tform_cam.position.z 581 | body_tform_cam_tf.rotation.x = body_tform_cam.rotation.x 582 | body_tform_cam_tf.rotation.y = body_tform_cam.rotation.y 583 | body_tform_cam_tf.rotation.z = body_tform_cam.rotation.z 584 | body_tform_cam_tf.rotation.w = body_tform_cam.rotation.w 585 | 586 | camera_transform_stamped = geometry_msgs.msg.TransformStamped() 587 | camera_transform_stamped.header.stamp = header.stamp 588 | camera_transform_stamped.header.frame_id = "base_link" 589 | camera_transform_stamped.transform = body_tform_cam_tf 590 | camera_transform_stamped.child_frame_id = img.source.name 591 | 592 | # Publish body to camera static tf 593 | spot_tf_static_broadcaster.sendTransform(camera_transform_stamped) 594 | 595 | # Publish current image and camera info 596 | image_only_pub.publish(i) 597 | camera_info_pub.publish(cam_info) 598 | 599 | ''' Publish occupancy grid''' 600 | if occupancy_grid_pub.get_num_connections() > 0: 601 | local_grid_proto = self.grid_client.get_local_grids(['terrain']) 602 | markers = get_terrain_markers(local_grid_proto) 603 | occupancy_grid_pub.publish(markers) 604 | 605 | rospy.logdebug("Looping...") 606 | rate.sleep() 607 | 608 | finally: 609 | # If we successfully acquired a lease, return it. 610 | self.lease_client.return_lease(self.lease) 611 | 612 | if __name__ == '__main__': 613 | """Command line interface.""" 614 | parser = argparse.ArgumentParser() 615 | bosdyn.client.util.add_common_arguments(parser) 616 | parser.add_argument('--motors_on', help='Power on motors [Y/n]', default="Y") 617 | options = parser.parse_args(sys.argv[1:]) 618 | 619 | try: 620 | robot = SpotInterface(options) 621 | robot.start_spot_ros_interface() 622 | except rospy.ROSInterruptException: 623 | pass --------------------------------------------------------------------------------