3

I'm trying to write a python code, that controls a drone. I receive the position from a Rigid-body trough a rostopic, and I want to use that data in my code. How can i access it in my python code?

#!/usr/bin/env python

import numpy as np
from pycrazyswarm import *

Z = 1.0

if __name__ == "__main__":
    swarm = Crazyswarm()

    # get initial position for maneuver
    swarm.allcfs.takeoff(targetHeight=Z, duration=1.0+Z)
    swarm.timeHelper.sleep(1.5+Z)

    print("press button to continue...")
    swarm.input.waitUntilButtonPressed()
    
    # After the button is pressed, I want, that the drone is aligned by a rigid body.
    # Means if the rigid body moves 1m left the drone should follow

    # finished following the rigid body. Get back landing
    swarm.allcfs.land(targetHeight=0.02, duration=1.0+Z)
    swarm.timeHelper.sleep(1.0+Z)

So after the button is pressed, I would like to use the data of the rostopic. On the host i send the data over the VRPN client of ROS http://wiki.ros.org/vrpn_client_ros I want to compute the data of the "tracker name"/pose topic

J.P.S.
  • 485
  • 4
  • 11
flrnhbr1
  • 33
  • 1
  • 4
  • This question might be put too simple regarding its depth. Or the answer is indeed very easy. Did you go through the [ROS Subscriber tutorial for Python](http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28python%29#rospy_tutorials.2FTutorials.2FWritingPublisherSubscriber.Writing_the_Subscriber_Node) already? – J.P.S. Mar 17 '22 at 17:37
  • I am very new to ROSYes i looked trough this, but if I unterstand it right, this is how you Build Subscriber but i wann to Access the data directly in the python Script and use it there. – flrnhbr1 Mar 17 '22 at 18:23
  • Question needs some code: Please provide enough code so others can better understand or reproduce the problem: https://stackoverflow.com/help/minimal-reproducible-example – D.L Mar 17 '22 at 18:32
  • Sorry you are right. – flrnhbr1 Mar 17 '22 at 18:59
  • By *compute*, do you mean *process*? If that's the case, I'd say you can't go that way (easily). Messages from ROSTopics are processed via Subscribers. Otherwise, well: Basically, you'd need to open a socket to the Publishers topic (in this case, each VRPN topic), You can get the IP and Port from the ROS master. Then, you need to read the data from it on a regular basis. So, you could try to go through hell by reimplementing the wheel and writing your own loop that listens to the network socket for incoming messages to process them. Is that, what you're aiming at? I can't recommend it, – J.P.S. Mar 17 '22 at 21:29
  • If i use rostopic echo /vrpn_client_ros/RigidBody/pose in the shell, I get the pose and orientation on the standard output. So my idea was to access the data of the topic directly in python, convert it to a variable and calculate the relative distance to the drone. It was just an idea, and I was curious if that is possible. If its very complicated to get this running, I will search for another approach. – flrnhbr1 Mar 17 '22 at 21:56

1 Answers1

2

Yes, you can access the ROS topic data in your Python code. Take the following example:

#!/usr/bin/env python

import numpy as np
import rospy
from pycrazyswarm import *
from geometry_msgs.msg import Pose


Z = 1.0

def some_callback(msg):
    rospy.loginfo('Got the message: ' + str(msg))

if __name__ == "__main__":
    swarm = Crazyswarm()
 
    # get initial position for maneuver
    swarm.allcfs.takeoff(targetHeight=Z, duration=1.0+Z)
    swarm.timeHelper.sleep(1.5+Z)

    print("press button to continue...")
    swarm.input.waitUntilButtonPressed()
    
    #start ros node
    rospy.init_node('my_node')
    #Create a subscriber
    rospy.Subscriber('/vrpn_client_ros/RigidBody/pose', Pose, some_callback)

    # finished following the rigid body. Get back landing
    swarm.allcfs.land(targetHeight=0.02, duration=1.0+Z)
    swarm.timeHelper.sleep(1.0+Z)

This will create a ROS node, listen to the data coming from your topic, and print it out much like rostopic echo would.

BTables
  • 4,413
  • 2
  • 11
  • 30