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 ) .
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!