Sphinx / Gazebo / ROS communication

Hello!

As far as I know, as Sphinx is based on Gazebo, I can connect and control the drone using ROS packages (eg. bebop_autonomy does it for Bebop 2). However, in order to get the model state for other models than the drone (i.e. pedestrians, cars, static objects…) I find no easy way to do it.

One usual way is to use Gazebo “topics”, which publishes information about all the models in the simulation environment. I know it’s possible to connect it to ROS, but only if Gazebo is launched through ROS (roslaunch).

Is there a way to launch Sphinx through Gazebo? Alternatively, if I include the fwman plugin in a Gazebo .world and launch it using Gazebo, would it work as if I use Sphinx?

1 Like

Any updates on this?

There is no way to make sphinx be working with another version of gazebo. One good reason for this is that Sphinx comes with a customized version 7 of gazebo.

You can access gazebo topics using the tool “parrot-gz”.

Do you know any information on with regards to the plugin part of this question. Is there a way to connect Gazebo plugins to sphinx. When I try connect ROS plugins to Sphinx, I get errors. For example:

/usr/lib/x86_64-linux-gnu/gazebo-7/plugins/libCameraPlugin.so: undefined symbol: _ZNK6gazebo7sensors12CameraSensor6CameraEv

Hey,
I think my question is relevant to this thread. I am working on an Anafi drone model and also wanted to access the Gazebo topics from simulation in my python code. I am not familiar with ROS and am looking for a solution without requiring ROS. Could anyone advise on the best way to do it?

I am able to access the co-ordinates from the tlm-data-logger (‘worldPosition’). But these are shell commands and extracting their output into my code for a control loop is making the whole learning process slow.

Thanks!

Hi @gargivaidya,

Parrot-Sphinx is a customized version of Gazebo, and so writing any sort of plugin (especially ROS plugin) can be quite tricky. I spent quite a while trying to get this to work and was only able to get a very basic ROS plugin working (not saying it isn’t possible, but I am saying, please tag me in a reply if you get one working :stuck_out_tongue: ) .

My workaround was to use a python script to call the tml-data-logger in a separate thread and parse the output from that command. That way, I could for instance, access the drone’s position in my script. Below is a python file that gets both the position and orientation of the drone.

The code works by starting a separate thread which directs each line of the output from tml-data-logger to the process_output function. If it finds the line with the data you are looking for it saves the data. I got the queue from an example with Python threading. I couldn’t seem to get the thread to work without the queue and so hence the queue (Note: I am sure there are many better ways to write this code, and maybe even better ways to achieve what you want). However, at least this is a starting point to accessing the position and orientation of the drone in a Python script.

#!/usr/bin/env python3
from subprocess import PIPE, Popen
from threading  import Thread
import sys
import numpy as np
import re
from queue import Queue, Empty

# Process the output from the file
def process_output(out, queue):
    for line in iter(out.readline, b''):
        line = str(line)
        if ".worldPosition" in line:
            number = re.findall(r"[-+]?\d*\.\d+|\d+", line)[0]
            if ".x" in line:
                pos1[0] = float(number)
            if ".y" in line:
                pos1[1] = float(number)
            if ".z" in line:
                pos1[2] = float(number)


        if ".worldAttitude" in line:
            number = re.findall(r"[-+]?\d*\.\d+|\d+", line)[0]
            if ".x" in line:
                att1[0] = float(number)
            if ".y" in line:
                att1[1] = float(number)
            if ".z" in line:
                att1[2] = float(number)
                
        queue.put(line)
    out.close()


if __name__ == "__main__":
    pos1 = np.zeros(3)
    att1 = np.zeros(3)
    q = Queue()

    # Run the command
    ON_POSIX = 'posix' in sys.builtin_module_names
    command = "tlm-data-logger -r 0 inet:127.0.0.1:9060"
    p = Popen(command, stdout=PIPE, bufsize=1, close_fds=ON_POSIX, shell=True)
    
    # Create a thread which dies with main program
    t = Thread(target=process_output, args=(p.stdout, q))
    t.daemon = True 
    t.start()

    for i in range(10000):
        try:  
            line = q.get_nowait()
        except Empty:
            # Clear out the queue
            q.queue.clear()

        print("Position: {}, {}, {}".format(pos1[0], pos1[1], pos1[2]))
        print("Orientation: {}, {}, {}".format(att1[0], att1[1], att1[2]))

    print("System Exiting\n")
    sys.exit(0)

With regards to @slekieffre’s answer about parrot-gz. I just did some tests and think you could easily change my above script to work using that too. For example I ran the following command in order to get the drones position using that:

$ parrot-gz topic -e /gazebo/default/pose/info | grep -A 12 'name: "anafi4k"'
>>> name: "anafi4k"
>>>   id: 10
>>>   position {
>>>     x: 4.6969254066515226e-05
>>>     y: 9.9998427788884268
>>>     z: 0.019893045253986885
>>>   }
>>>   orientation {
>>>     x: 1.7804686170798035e-05
>>>     y: -3.5485437607453444e-06
>>>     z: -0.000754391957539744
>>>     w: 0.99999967653665167
>>>   }

I hope this helps!

1 Like

Thank you so much @Watchdog101 for sharing your code and your insights on this topic. Your code worked for me except for a small modification -

'number = re.findall(r"[-+]?\d*\.\d+|\d+", line)[1]' instead of 'number = re.findall(r"[-+]?\d*\.\d+|\d+", line)[0]'

The output for your code was >> Position: 4.0, 4.0, 4.0 towards the end of the queue (which is incorrect)
I modified this command to get >> Position: 0.011956, -0.0002, 0.037693
which was the desired worldPosition format I wanted.

However, the tlm-data-logger throws the following error after several time-steps of my RL code (even if tlm-data-logger is executed from the console)

E pomp: read(fd=7) err=104(Connection reset by peer)

So, I have shifted to using ‘parrot-gz’ instead, to extract the ‘anafi4k position’ as feedback.

Is there any additional documentation on parrot-gz besides the ‘Advanced Topics’ page? It seems a very useful tool, since we can make do without ROS. I am now looking to control the drone by publishing to the topics.

Thanks!

Thanks for the feedback. I don’t want to go too far off topic, but to answer your question I am not sure of a better place to find documentation on parrot-gz.

The best way I have found to control the drone is through using Olympe. It works pretty well and there is quite a bit of documentation and example code which you can play with.

1 Like

Thanks @Watchdog101! Yes, I am currently using Olympe but I was looking to speed up my simulation, since Olympe commands seem to be taking longer to execute.

I am not sure what you mean by taking longer to execute. If you are having issues with simulation speed when using Olympe I would suggest moving that to a new question :slight_smile: