├── .gitignore ├── .gitmodules ├── GazeboMotionCapture └── __init__.py ├── LICENSE ├── README.md ├── demo.py ├── setup.cfg └── setup.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | dist/ 3 | build/ 4 | GazeboMotionCapture.egg-info/ 5 | .DS_Store 6 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "pygazebo"] 2 | path = pygazebo 3 | url = https://github.com/VimDrones/pygazebo.git 4 | -------------------------------------------------------------------------------- /GazeboMotionCapture/__init__.py: -------------------------------------------------------------------------------- 1 | from threading import Thread 2 | import trollius 3 | from trollius import From 4 | import pygazebo 5 | from pygazebo.msg import poses_stamped_pb2 6 | from pyquaternion import Quaternion 7 | from math import pi 8 | 9 | loop = trollius.get_event_loop() 10 | 11 | EXIT = False 12 | def exit(): 13 | EXIT = True 14 | loop.stop() 15 | print("exiting... motion captrue") 16 | 17 | def parse_data(data): 18 | message = poses_stamped_pb2.PosesStamped.FromString(data) 19 | 20 | position = message.pose[0].position 21 | position = (position.y, position.x, position.z) 22 | 23 | orientation = message.pose[0].orientation 24 | 25 | q = Quaternion(orientation.w, orientation.x, orientation.y * -1.0, orientation.z * -1.0) 26 | convert_q = tuple(q*Quaternion(axis=[0, 0, 1], radians=pi * 0.5)) 27 | 28 | callback(1, position, convert_q) 29 | 30 | @trollius.coroutine 31 | def subscribe_loop(address): 32 | manager = yield From(pygazebo.connect(address=address)) 33 | 34 | manager.subscribe('/gazebo/default/pose/info', 35 | 'gazebo.msgs.PosesStamped', 36 | parse_data) 37 | 38 | while not EXIT: 39 | yield From(trollius.sleep(0.2)) 40 | 41 | def loop_in_thread(loop, address): 42 | try: 43 | trollius.set_event_loop(loop) 44 | loop.run_until_complete(subscribe_loop(address)) 45 | except Exception as e: 46 | print(e) 47 | 48 | def create_motion_capture(address): 49 | motion_capture = Thread(target=loop_in_thread, args=(loop, address,)) 50 | motion_capture.exit = exit 51 | return motion_capture 52 | 53 | class PositionEstimate(object): 54 | 55 | def __init__(self): 56 | self.positions_buffer = [] 57 | self.delay = 0.05 58 | self.init = False 59 | self.first_position = [] 60 | self.estimated_position = [] 61 | 62 | def is_ready(self): 63 | return len(self.estimated_position) > 0 64 | 65 | def receive_new_position(self, x, y, z): 66 | new_position = [x, y, z] 67 | if not self.init: 68 | self.first_position = new_position[:] 69 | self.init = True 70 | 71 | self.positions_buffer.append(new_position) 72 | 73 | if len(self.positions_buffer) > 1: 74 | self.positions_buffer = self.positions_buffer[-2:] 75 | position = [] 76 | for i in range(3): 77 | position.append((self.positions_buffer[-1][i] - self.positions_buffer[-2][i])*self.delay + self.positions_buffer[-1][i]) 78 | self.estimated_position = position 79 | 80 | @property 81 | def ned_position(self): 82 | x, y, z = self.estimated_position 83 | x0, y0, z0 = self.first_position 84 | return x - x0, y - y0, z0 - z 85 | 86 | if __name__ == '__main__': 87 | def callback(id, position, orientation): 88 | print(id, position, orientation) 89 | motion_capture = create_motion_capture(('127.0.0.1', 11345), callback) 90 | motion_capture.start() 91 | 92 | import time 93 | time.sleep(5) 94 | motion_capture.exit() 95 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 PixelBatStudio 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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # GazeboMotionCapture 2 | 3 | Motion Capture System Simulator Base on Gazebo, can be use for vehicle position Estimate 4 | 5 | ### install 6 | 7 | ```shell 8 | pip install GazeboMotionCapture 9 | ``` 10 | 11 | ### dev 12 | 13 | ```shell 14 | git submodule update --init --recursive 15 | ``` 16 | 17 | ### usage 18 | 19 | ```python 20 | #example with dronekit 21 | print "Start simulator (SITL)" 22 | import dronekit_sitl 23 | sitl = dronekit_sitl.start_default() 24 | connection_string = sitl.connection_string() 25 | 26 | # Import DroneKit-Python 27 | from dronekit import connect, VehicleMode 28 | 29 | # Connect to the Vehicle. 30 | print("Connecting to vehicle on: %s" % (connection_string,)) 31 | vehicle = connect(connection_string, wait_ready=True) 32 | 33 | import time 34 | import GazeboMotionCapture 35 | position_estimate = GazeboMotionCapture.PositionEstimate() 36 | 37 | def callback(id, position, orientation): 38 | print(id, position, orientation) 39 | if not position_estimate.init: 40 | position_estimate.first_position = position 41 | position_estimate.init = True 42 | 43 | position_estimate.receive_new_position(position[0], position[1], position[2]) 44 | if position_estimate.is_ready(): 45 | msg = vehicle.message_factory.att_pos_mocap_encode( 46 | int(round(time.time() * 1000)), 47 | orientation, 48 | position_estimate.ned_position[0], 49 | position_estimate.ned_position[1], 50 | position_estimate.ned_position[2]) 51 | vehicle.send_mavlink(msg) 52 | 53 | 54 | GazeboMotionCapture.callback = callback 55 | motion_capture = GazeboMotionCapture.create_motion_capture(('192.168.1.49', 11345)) 56 | 57 | motion_capture.start() 58 | 59 | time.sleep(10) 60 | motion_capture.exit() 61 | ``` 62 | 63 | -------------------------------------------------------------------------------- /demo.py: -------------------------------------------------------------------------------- 1 | print "Start simulator (SITL)" 2 | import dronekit_sitl 3 | sitl = dronekit_sitl.start_default() 4 | connection_string = sitl.connection_string() 5 | 6 | # Import DroneKit-Python 7 | from dronekit import connect, VehicleMode 8 | 9 | # Connect to the Vehicle. 10 | print("Connecting to vehicle on: %s" % (connection_string,)) 11 | vehicle = connect(connection_string, wait_ready=True) 12 | 13 | import time 14 | import GazeboMotionCapture 15 | position_estimate = GazeboMotionCapture.PositionEstimate() 16 | 17 | def callback(id, position, orientation): 18 | print(id, position, orientation) 19 | if not position_estimate.init: 20 | position_estimate.first_position = position 21 | position_estimate.init = True 22 | 23 | position_estimate.receive_new_position(position[0], position[1], position[2]) 24 | if position_estimate.is_ready(): 25 | msg = vehicle.message_factory.att_pos_mocap_encode( 26 | int(round(time.time() * 1000)), 27 | orientation, 28 | position_estimate.ned_position[0], 29 | position_estimate.ned_position[1], 30 | position_estimate.ned_position[2]) 31 | vehicle.send_mavlink(msg) 32 | 33 | 34 | GazeboMotionCapture.callback = callback 35 | motion_capture = GazeboMotionCapture.create_motion_capture(('192.168.1.49', 11345)) 36 | 37 | motion_capture.start() 38 | 39 | time.sleep(10) 40 | motion_capture.exit() 41 | -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [metadata] 2 | description-file = README.md 3 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | """Motion Capture System Simulator Base on Gazebo 2 | See: 3 | https://github.com/PixelBatStudios/GazeboMotionCapture 4 | """ 5 | 6 | from setuptools import setup, find_packages 7 | from codecs import open 8 | from os import path 9 | 10 | here = path.abspath(path.dirname(__file__)) 11 | 12 | 13 | setup( 14 | name='GazeboMotionCapture', 15 | 16 | # Versions should comply with PEP440. For a discussion on single-sourcing 17 | # the version across setup.py and the project code, see 18 | # https://packaging.python.org/en/latest/single_source_version.html 19 | version='0.1.3', 20 | 21 | description='Motion Capture System Simulator Base on Gazebo, can be use for vehicle position Estimate', 22 | long_description="Motion Capture System Simulator Base on Gazebo, can be use for vehicle position Estimate, like Drone and rover", 23 | 24 | # The project's main homepage. 25 | download_url='', 26 | url='https://github.com/PixelBatStudios/GazeboMotionCapture', 27 | 28 | # Author details 29 | author='Huibean Luo', 30 | author_email='huibean@users.noreply.github.com', 31 | 32 | # Choose your license 33 | license='MIT', 34 | 35 | # See https://pypi.python.org/pypi?%3Aaction=list_classifiers 36 | classifiers=[ 37 | # How mature is this project? Common values are 38 | # 3 - Alpha 39 | # 4 - Beta 40 | # 5 - Production/Stable 41 | 'Development Status :: 4 - Beta', 42 | 43 | # Indicate who your project is intended for 44 | 'Intended Audience :: Developers', 45 | 'Topic :: Software Development :: Embedded Systems', 46 | 'Topic :: Software Development :: Libraries :: Python Modules', 47 | 'Topic :: Scientific/Engineering :: Mathematics', 48 | 'Topic :: Scientific/Engineering :: Physics', 49 | 'Topic :: Scientific/Engineering :: Visualization', 50 | 51 | 52 | # Pick your license as you wish (should match "license" above) 53 | 'License :: OSI Approved :: MIT License', 54 | 55 | # Specify the Python versions you support here. In particular, ensure 56 | # that you indicate whether you support Python 2, Python 3 or both. 57 | 58 | 'Programming Language :: Python :: 2.7', 59 | 'Programming Language :: Python :: 3', 60 | 'Programming Language :: Python :: 3.1', 61 | 'Programming Language :: Python :: 3.2', 62 | 'Programming Language :: Python :: 3.3', 63 | 'Programming Language :: Python :: 3.4', 64 | 'Programming Language :: Python :: 3.5' 65 | ], 66 | 67 | 68 | 69 | # What does your project relate to? 70 | keywords=[ 71 | 'motion capture', 'vicon', 'optitrack', 'indoor positioning', 'px4', 'ardupilot' 72 | ], 73 | 74 | # You can just specify the packages manually here if your project is 75 | # simple. Or you can use find_packages(). 76 | packages=['GazeboMotionCapture'], 77 | 78 | # Alternatively, if you want to distribute just a my_module.py, uncomment 79 | # this: 80 | #py_modules=["GazeboMotionCapture"], 81 | 82 | # List run-time dependencies here. These will be installed by pip when 83 | # your project is installed. For an analysis of "install_requires" vs pip's 84 | # requirements files see: 85 | # https://packaging.python.org/en/latest/requirements.html 86 | install_requires=["numpy", "trollius", "pygazebo", "pyquaternion"], 87 | 88 | # List additional groups of dependencies here (e.g. development 89 | # dependencies). You can install these using the following syntax, 90 | # for example: 91 | # $ pip install -e .[dev,test] 92 | extras_require={ 93 | }, 94 | 95 | # If there are data files included in your packages that need to be 96 | # installed, specify them here. If using Python 2.6 or less, then these 97 | # have to be included in MANIFEST.in as well. 98 | package_data={ 99 | }, 100 | 101 | # Although 'package_data' is the preferred approach, in some case you may 102 | # need to place data files outside of your packages. See: 103 | # http://docs.python.org/3.4/distutils/setupscript.html#installing-additional-files # noqa 104 | # In this case, 'data_file' will be installed into '/my_data' 105 | data_files=[], 106 | 107 | # To provide executable scripts, use entry points in preference to the 108 | # "scripts" keyword. Entry points provide cross-platform support and allow 109 | # pip to create the appropriate form of executable for the target platform. 110 | entry_points={}, 111 | 112 | # Use nose to discover all tests in the module 113 | test_suite='nose.collector', 114 | 115 | # Set Nose as a requirement for running tests 116 | tests_require=['nose'], 117 | ) 118 | --------------------------------------------------------------------------------