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
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.
Related
I am still struggling with using data from the callback function, I wrote a class and trying to get the data from the callback function associated with the class, any help would be much appreciated. Do I need to use multi threading or there is an easy way to use it ? When I am calling the class the publisher initializes and then callback keep getting the updated data but I am not sure how can I use this data.
#!/usr/bin/env python3
"""OpenCV feature detectors with ros CompressedImage Topics in python.
This example subscribes to a ros topic containing sensor_msgs
CompressedImage. It converts the CompressedImage into a numpy.ndarray,
then detects and marks features in that image. It finally displays
and publishes the new image - again as CompressedImage topic.
"""
__version__= '0.1'
from moveit_commander.conversions import pose_to_list
from rospy_tutorials.msg import Floats
from rospy.numpy_msg import numpy_msg
from tf import TransformListener
from std_msgs.msg import String
import geometry_msgs.msg
import moveit_commander
import moveit_msgs.msg
from math import pi
import sys, time
import rospy
import copy
VERBOSE=False
class move_xarm:
# global my_data
def __init__(self):
'''Initialize ros publisher and subscriber'''
# publish trajectories for RViz to visualize
self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory,queue_size=20)
self.data_callback_publisher = rospy.Publisher('/callback_data',numpy_msg(Floats),queue_size=1)
self.my_data = None
self.listener()
# subscribed Topic
# self.subscriber = rospy.Subscriber("marker_wrt_base_pose", numpy_msg(Floats), self.callback, queue_size = 1)
if VERBOSE :
print("subscribed to marker_wrt_base_pose")
def listener(self):
# In ROS, nodes are uniquely named. If two nodes with the same
# name are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
rospy.Subscriber("marker_wrt_base_pose", numpy_msg(Floats), self.callback,queue_size = 1)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
def callback(self, ros_data):
'''Callback function of subscribed topic.
Here position data get converted into float64'''
if VERBOSE :
print('received data of type: "%s"' % ros_data.format)
self.my_data = ros_data.data
# my_data = my_data.astype('float64')
self.data_callback_publisher.publish(ros_data)
if __name__ == '__main__':
mv = move_xarm()
I resolved this issue,
Create another function out of the class say name use_data(args)
Call use_data in main function and you are good to go.
One of the common problem is use of spin command,they block the main thread from exiting until ROS invokes a shutdown - via a Ctrl + C.
def use_data(args):
ic = move_xarm()
rospy.init_node('move_group_python_interface_tutorial',anonymous=True)
rate = rospy.Rate(5) # ROS Rate at 5Hz
while not rospy.is_shutdown():
do stuff using the variable ic.my_data
rate.sleep()
Using DHT22 sensor, display LCD 16x2 and raspberry pi to collect temperature and humidity value.Please help me out with this error.Thanks in advance
import drivers # Imports Custom LCD Display Driver
import socket # Imports socket function
import struct # Imports structure function
import fcntl # Imports networking function
import time # Imports time function
import os # Imports Operating System function
import re # Imports Reg Ex function
from time import sleep # Imports sleep function from time module
import Adafruit_DHT
DHT = 4
disp = drivers.Lcd() # Initializes LCD Display
def tempcpu(): #Defines "tempcpu"
for _ in range(5): # Sets up timer
cputemp = os.popen("vcgencmd measure_temp").readline()
celsius = re.sub("[^0123456789\.]", "", cputemp)
h,t = Adafruit_DHT.read_retry(Adafruit_DHT.DHT22, DHT)
disp.lcd_display_string("CpuRpi: {}C".format(celsius), 1)
disp.lcd_display_string("T={0:0.1f}C H={1:0.1f}%".format(t,h), 2 )
sleep(1) # Sleeps for one second before restarting loop
while True: # Forever loop
tempcpu() # Calls "tempcpu"
disp.lcd_clear() # Clears the LCD Display
except KeyboardInterrupt: # If interrupted by the keyboard ("Control" + "C")
disp.clear() #clear the lcd display
sleep(1) #sleeps 1 second
disp.backlight(0) #Turn Off Backlight
# Exits the python interpreter
exit()
Error :
disp.lcd_display_string("T={0:0.1f}C H={1:0.1f}%".format(t,h), 2 )
type Error:unsupported format string passed to NoneType.format
I can reproduce your error with the following line (or similarly if t is the one set to None):
h = None,
t = 1
str_to_be_printed = "T={0:0.1f}C H={1:0.1f}%".format(t,h)
The method Adafruit_DHT.read_retry returned None. It probably happens when failing to read the sensor values. Maybe there is an error in your circuit, or the sensor is faulty.
More complete example on https://www.programcreek.com/python/example/92777/Adafruit_DHT.read_retry
I am fairly new to Python and Computer Vision but I've managed to setup a basic script to open my CCTV camera's at home. It works great but I have a slight issue is is that it loops over each camera one after the other, so instead of the camera image updating every second, it updates around every 5 seconds - the time it takes to complete a loop.
What I want is to be able to use multiprocessing / multi threading so that when I call the function for each of my cameras it open ins a new pool, running parallel to each other, therefore updating each camera every second.
As you can see in the below code I call each of the cameras using the same function, but with different argurments. I've read up on Process and have tried a few variations but I don't seem to be able to get it working.
I'm sure its a simple one and it would be great if someone could point me in the right direction.
Here's the code:
# import libraries
from threading import Thread
import imutils
import cv2, time
import argparse
import numpy as np
import datetime
from imutils.video import WebcamVideoStream
from multiprocessing import Process
stream = WebcamVideoStream('rtsp://mylink1./' ).start() # To open any valid video file stream/network stream. In this case 'test.mp4' file.
stream2 = WebcamVideoStream('rtsp://mylink2./' ).start()
stream3 = WebcamVideoStream('rtsp://mylink3./' ).start()
stream4 = WebcamVideoStream('rtsp://mylink4./' ).start()
stream5 = WebcamVideoStream('rtsp://mylink5./' ).start()
def checkimage(stream,camname):
global lastmessage
try:
frame = stream.read()
(h, w) = frame.shape[:2]
cv2.imshow(camname, frame)
print('[INFO]Checked ' + str(camname) + ' at ' + datetime.datetime.now().strftime("%H:%M:%S") + '...')
except AttributeError:
pass
# infinite loop
while True:
checkimage(stream,"Back Door Camera")
checkimage(stream2,"Conservatory Camera")
checkimage(stream3,"Front Door Camera")
checkimage(stream4,"Garage Camera")
checkimage(stream5,"Shed Camera")
key = cv2.waitKey(1) & 0xFF
# check for 'q' key-press
if key == ord("q"):
if 'q' key-pressed break out
break
cv2.destroyAllWindows()
# close output window
stream.stop()
# safely close video stream.
Thanks in advance!
Chris
I am currently trying to make a ROS node in Python which has both a subscriber and a publisher.
I've seen examples where a message is published within the callback, but I want it to "constantly" publish messages, and perform callbacks when it is the case.
Here is how I do it now:
#!/usr/bin/env python
import rospy
from std_msgs.msg import Empty
from std_msgs.msg import String
import numpy as np
pub = rospy.Publisher('/status', String, queue_size=1000)
def callback(data):
print "Message received"
def listener():
rospy.init_node('control', anonymous=True)
rospy.Subscriber('control_c', Empty, callback)
rospy.spin()
if __name__ == '__main__':
print "Running"
listener()
So where should I publish?
Well, I think there's a lot of solutions here, you could even make use of a python process, but what I'm proposing is a ROS approach using a ros Timer.
I am not really that efficient in python but this code may gave you a heads up.
#!/usr/bin/env python
import rospy
from std_msgs.msg import Empty
from std_msgs.msg import String
import numpy as np
last_data = ""
started = False
pub = rospy.Publisher('/status', String, queue_size=1000)
def callback(data):
print "New message received"
global started, last_data
last_data = data
if (not started):
started = True
def timer_callback(event):
global started, pub, last_data
if (started):
pub.publish(last_data)
print "Last message published"
def listener():
rospy.init_node('control', anonymous=True)
rospy.Subscriber('control_c', String, callback)
timer = rospy.Timer(rospy.Duration(0.5), timer_callback)
rospy.spin()
timer.shutdown()
if __name__ == '__main__':
print "Running"
listener()
Here, your callback will update the message and your timer will fire up every 0.5sec and publishes the last data received.
you can test this code by publishing data on "/contriol_c" every 3 seconds and configuring you timer to 0.5 sec. start an echo on /status
$ rostopic echo /status
and you'll got your message published on a 2 Hz rate.
Hope that helps !
Simply replace rospy.spin() with the following loop:
while not rospy.is_shutdown():
# do whatever you want here
pub.publish(foo)
rospy.sleep(1) # sleep for one second
Of course you can adjust the sleep duration to whatever value you want (or even remove it entirely).
According to this reference subscribers in rospy are running in a separate thread, so you don't need to call spin actively.
Note that in roscpp (i.e. when using C++) this is handled differently. There you have to call ros::spinOnce() in the while loop.
I have written this little script to show current track playing on xmms2 on a notification widget using xmms client and pynotify, so when i run it i can see the widget popup with current artist and title using xmmsclient methods.
Can anybody give some hints about how to detect track change to notify automatically without having to run the script manually?
You connect the client library to a main loop, and register as a listener via the broadcast_ playback_current_id method. If you want the currently playing id when the script starts as well you can call the playback_current_id method.
Here is a small adaptation of tutorial6 in the xmms2-tutorial.git which uses the GLib Mainloop to drive the connection:
import xmmsclient
import xmmsclient.glib
import os
import sys
import gobject
def cb(result):
if not result.is_error():
print "Current: %(artist)s - %(title)s" % result.value()
ml = gobject.MainLoop(None, False)
xc = xmmsclient.XMMS("stackoverflow")
xc.connect()
conn = xmmsclient.glib.GLibConnector(xc)
xc.broadcast_playback_current_id(lambda r: xc.medialib_get_info(r.value(), cb))
ml.run()