vimdrones / gazebomotioncapture

Motion Capture System Simulator Base on Gazebo8, can be use for vehicle SITL position Estimate

GazeboMotionCapture

Motion Capture System Simulator Base on Gazebo, can be use for vehicle position Estimate

install

pip install GazeboMotionCapture

dev

 git submodule update --init --recursive

usage

#example with dronekit
print "Start simulator (SITL)"
import dronekit_sitl
sitl = dronekit_sitl.start_default()
connection_string = sitl.connection_string()

# Import DroneKit-Python
from dronekit import connect, VehicleMode

# Connect to the Vehicle.
print("Connecting to vehicle on: %s" % (connection_string,))
vehicle = connect(connection_string, wait_ready=True)

import time
import GazeboMotionCapture
position_estimate = GazeboMotionCapture.PositionEstimate()

def callback(id, position, orientation):
    print(id, position, orientation)
    if not position_estimate.init:
        position_estimate.first_position = position
        position_estimate.init = True

    position_estimate.receive_new_position(position[0], position[1], position[2])
    if position_estimate.is_ready():
        msg = vehicle.message_factory.att_pos_mocap_encode(
            int(round(time.time() * 1000)),
            orientation,
            position_estimate.ned_position[0],
            position_estimate.ned_position[1],
            position_estimate.ned_position[2])
        vehicle.send_mavlink(msg)


GazeboMotionCapture.callback = callback
motion_capture = GazeboMotionCapture.create_motion_capture(('192.168.1.49', 11345))

motion_capture.start()

time.sleep(10)
motion_capture.exit()

gazebomotioncapture's Issues

how to use this package for ground rover?

Hi,

I am simulating a ground robot along with other objects in gazebo. I want to use a motion capture system that provides me with its position and orientation. How can I use this package? I am confused how to use the IP address in this case?

pleas help.

Demo.py runtime issue

Hello,
I am trying to run demo.py in conjunction with PX4 SITL w/ Gazebo. I edited demo.py to reflect the IP address of my machine.
GazeboMotionCapture.callback = callback
motion_capture = GazeboMotionCapture.create_motion_capture(('192.168.1.186', 11345))
I did also try 127.0.0.1 as well, and ensured the port was correct. However, I get the following runtime error:

Start simulator (SITL)
Starting copter simulator (SITL)
SITL already Downloaded and Extracted.
Ready to boot.
Connecting to vehicle on: tcp:127.0.0.1:5760
CRITICAL:autopilot:APM:Copter V3.3 (d6053245)
CRITICAL:autopilot:Frame: QUAD
CRITICAL:autopilot:Calibrating barometer
CRITICAL:autopilot:Initialising APM...
CRITICAL:autopilot:barometer calibration complete
CRITICAL:autopilot:GROUND START
(1, (0.10450192945681276, 0.9800527032718428, 0.0), (0.70711233085285752, 0.00074889015951397125, 0.00021415853659370002, 0.70710080247125251))
ERROR:trollius:Exception in callback (<Future ...ff\xef?'>) at /usr/local/lib/python2.7/dist-packages/pygazebo-3.0.0_2014.1-py2.7.egg/pygazebo/pygazebo.py:198
handle: <Handle (<Future ...ff\xef?'>) at /usr/local/lib/python2.7/dist-packages/pygazebo-3.0.0_2014.1-py2.7.egg/pygazebo/pygazebo.py:198>
Traceback (most recent call last):
File "/usr/local/lib/python2.7/dist-packages/trollius-2.2.post1-py2.7.egg/trollius/events.py", line 136, in _run
self._callback(*self._args)
File "/usr/local/lib/python2.7/dist-packages/pygazebo-3.0.0_2014.1-py2.7.egg/pygazebo/pygazebo.py", line 198, in
lambda future: self._handle_read(future, connection))
File "/usr/local/lib/python2.7/dist-packages/pygazebo-3.0.0_2014.1-py2.7.egg/pygazebo/pygazebo.py", line 209, in _handle_read
self._connect3(connection)
TypeError: _connect3() takes exactly 3 arguments (2 given)
exiting... motion captrue
Event loop stopped before Future completed.

I've ensured that all dependencies have been installed correctly, and that my PX4 SITL/Gazebo is running properly. Any advice on this would be greatly appreciated. Thank you!