I was doing in a project where i have to take values from two subscribers then fulfill some conditions and then publish data. Im using classes to values for the two subscribers. However, whenever In the attribute move_bot i call self.ranges and self.linear_pose which should work as they have been set when the code subscribed to them. However, i have to call the move_bot attribute within the laser_callback to make it work but now it only recognizes the self.ranges and not the self.linear_pose. If i do place the move_bot in the odom_callback the same thing happens but with self.linear_pose. Why are the callbacks not working?
import rospy
import math
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
class Move(object):
def __init__(self):
rospy.init_node('robot_maze')
rospy.Subscriber('/kobuki/laser/scan',LaserScan,self.laser_callback)
rospy.Subscriber('/odom',Odometry,self.odom_callback)
self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
self.twist_angle = Twist()
def laser_callback(self,msg):
self.ranges = msg.ranges
def odom_callback(self,msg):
self.linear_pose = msg
def move_bot(self):
pi = math.pi
kp = -0.027
val =[]
if self.linear_pose.twist.twist.linear.x < 0.001:
self.twist_angle.linear.x = -0.2
for i in self.ranges:
if i <= 0.6:
if self.ranges.index(i) > 360:
self.twist_angle.angular.z = 1
else:
self.twist_angle.angular.z = -1
break
elif self.ranges[360] > 1.2:
self.twist_angle.angular.z = 0
self.twist_angle.linear.x = 0.5
else:
for i in self.ranges:
if i > 1.2:
val.append(i)
angle = self.ranges.index(max(val))
print(max(val))
if self.ranges[angle] > self.ranges[360]:
self.twist_angle.angular.z = kp * (360 - angle)
self.pub.publish(self.twist_angle)
if __name__== '__main__':
try:
move = Move()
move.move_bot()
rate = rospy.Rate(10)
except rospy.ROSInterruptException: pass
while not rospy.is_shutdown():
rate.sleep()
I get the following error message
Traceback (most recent call last):
File "/home/user/catkin_ws/src/robot_maze/src/robot_maze.py", line 100, in <module>
move.move_bot()
File "/home/user/catkin_ws/src/robot_maze/src/robot_maze.py", line 45, in move_bot
if self.ranges[360] > 1.2:
AttributeError: 'Move' object has no attribute 'ranges'
[robot_maze-1] process has died [pid 16344, exit code 1, cmd
/home/user/catkin_ws/src/robot_maze/src/robot_maze.py
__name:=robot_maze __log:=/home/user/.ros/log/e4216cee-9f0e-11ea-9fd8-06b6df32753c/robot_maze-
1.log].
log file: /home/user/.ros/log/e4216cee-9f0e-11ea-9fd8-06b6df32753c/robot_maze-1*.log
To point out what #a_guest said in the comments:
class Move(object):
def __init__(self):
...
self.ranges = None
self.linear_pose = None
...
def move_bot(self):
if self.ranges is not None and self.linear_pose is not None:
if self.linear_pose.twist.twist.linear.x < 0.001:
...
elif self.ranges[360] > 1.2:
...
else:
...
self.pub.publish(self.twist_angle) # Don't publish until received initial values
Related
I'm creating FPS game with Ursina Engine (python). But I started to get a error whenever I tried to play it.
I just started coding so I don't know what I should do.
Code:
from ursina import *
from ursina.prefabs.first_person_controller import *
class Player(Entity):
def __init__(self, **kwargs):
self.controller = FirstPersonController(**kwargs)
super().__init__(parent=self.controller)
self.hand_gun = Entity(parent=self.controller.camera_pivot, scale=0.1, position=Vec3(0.7, -1, 1.5), rotation=Vec3(0, 170, 0), model='gun', Texture='M1911-RIGHT', visible=False)
self.knife = Entity(parent=self.controller.camera_pivot, scale=0.4, position=Vec3(0.7, -1, 1.5), rotation=Vec3(0, 170, 0), model='knife', Texture='knife', visible=False)
self.weapons = [self.hand_gun, self.knife]
self.current_weapon = 0
self.switch_weapons()
def switch_weapon(self):
for i, v in enumerate(self.weapons):
if i == self.current_weapon:
v.visible = True
else:
v.visible = False
def input(self, key):
try:
self.current_weapon = int(key) - 1
self.switch_weapon()
except ValueError:
pass
if key == 'scroll up':
self.current_weapon = (self.current_weapon + 1) % len(self.weapons)
self.switch_weapon()
if key == 'scroll down':
self.current_weapon = (self.current_weapon - 1) % len(self.weapons)
self.switch_weapon()
if key == 'left_mouse_down' and self.current_weapon == 0:
Bullet(model='sphere', color=color.black, scale=0.2, position=self.controller.camera_pivot.world_position, rotation=self.controller.camera_pivot.world_rotation)
def update(self):
self.controller.camera_pivot.y = 2 - held_keys['left control']
class Bullet(Entity):
def __init__(self, speed=50, lifetime = 10, **kwargs):
super().__init__(**kwargs)
self.speed = speed
self.lifetime = lifetime
self.start = time.time()
def update(self):
ray = raycast(self.world_position, self.forward, distance=self.speed*time.dt)
if not ray.hit and time.time() - self.start < self.lifetime:
self.world_position += self.forward * self.speed * time.daylight
else:
destroy(self)
app = Ursina
ground = Entity(model='plane', scale=20, texture='white_cube', texture_scale='mesh')
player = Player(position=(0,10,0))
app.run()
It's just small shooting game that I made for learning Ursina Engine
and the error message I got was:
package_folder: C:\Users\Yunwoo Chang\AppData\Roaming\Python\Python310\site-packages\ursina
asset_folder: c:\Users\Yunwoo Chang\Desktop\Tetris\Ursina Engine
Exception ignored in: <function Texture.__del__ at 0x000001E55A5129E0>
Traceback (most recent call last):
File "C:\Users\Yunwoo Chang\AppData\Roaming\Python\Python310\site-packages\ursina\texture.py", line 185, in __del__ del self._cached_image
AttributeError: _cached_image
Traceback (most recent call last):
File "c:\Users\Yunwoo Chang\Desktop\Tetris\Ursina Engine\03 - Weapons.py", line 71, in <module>
player = Player(position=(0,10,0))
File "c:\Users\Yunwoo Chang\Desktop\Tetris\Ursina Engine\03 - Weapons.py", line 6, in __init__
self.controller = FirstPersonController(**kwargs)
File "C:\Users\Yunwoo Chang\AppData\Roaming\Python\Python310\site-packages\ursina\prefabs\first_person_controller.py", line 32, in __init__
ray = raycast(self.world_position+(0,self.height,0), self.down, ignore=(self,))
File "C:\Users\Yunwoo Chang\AppData\Roaming\Python\Python310\site-packages\ursina\entity.py", line 433, in world_position
return Vec3(self.get_position(render))
NameError: name 'render' is not defined
How can I fix it?
I think it's some kind of attribute error but I don't know what to do.
You should instantiate Ursina with app = Ursina() before instantiating Entities. You're missing the ().
Hi I wrote a ROS Node in python which imports pyaudio for processing audio. I am using virtulenv with all python modules installed. I can run my Node in Pycharm however when i use rosrun mic_pkg mic_app.py. I'm getting following error
AttributeError: 'module' object has no attribute 'PyAudio'
Here is my node to run.
#!/usr/bin/env python
import pyaudio
import numpy as np
import struct
import rospy
from std_msgs.msg import Float64
class AudioInterface:
def __init__ (self):
self.NSAMPLES = 92000
self.CHUNK = 1024 * 4
self.FORMAT = 8
self.CHANNELS = 1
self.RATE = 92000 # Hz or 152000, 192000 max
self.RECORD_SECONDS = 3
self.FRAMES = []
self.interface = pyaudio.PyAudio ()
self.stream = self.interface.open (format=self.FORMAT,
channels=self.CHANNELS, rate=self.RATE, input=True,
frames_per_buffer=self.CHUNK)
self.stream.start_stream ()
self.set_device_index ()
def __del__ (self):
self.stream.stop_stream ()
self.interface.terminate ()
def set_device_index (self):
for i in range (self.interface.get_device_count ()):
devinfo = self.interface.get_device_info_by_index (i)
print ('Device %{}: %{}'.format (i, devinfo['name']))
for keyword in ['mic', 'input']:
if keyword in devinfo['name'].lower ():
print ('Found an input: device {} - {}'.format (i, devinfo['name']))
self.device_index = i
def get_chunks (self):
for i in range (0, int (self.RATE / self.CHUNK * self.RECORD_SECONDS)):
data = np.frombuffer (self.stream.read (self.CHUNK), dtype=np.int16)
data = data / 1000
self.FRAMES.append (data)
return self.FRAMES
def get_audio_block (self):
data = np.frombuffer (self.stream.read (self.RATE), dtype=np.int16)
data = data / 1000
return data
class AudioProcessing:
#staticmethod
def calc_fft (array):
fftdata = np.abs (np.fft.rfft (array))
return fftdata
#staticmethod
def calc_rms (array):
rms = np.sqrt (np.mean (array ** 2))
return rms
#staticmethod
def calc_mean (array):
result = np.mean (array)
return result
#staticmethod
def normalize (array):
buff = []
min_value = min (array)
max_value = max (array)
for x in array:
y = (x - min_value) / (max_value - min_value)
buff.append (y)
return buff
#staticmethod
def zeros (array):
for n in range (0, 38000):
array[n] = 0
for k in range (42000, len (array)):
array[k] = 0
return array
LEAK_TRIGGER = 0.25
is_leaking = False
trigger = False
if __name__ == "__main__":
mic = AudioInterface ()
is_leaking = False
rospy.init_node ('microphone_node', anonymous=False)
pub = rospy.Publisher ('mic_data', Float64, queue_size=10)
rate = rospy.Rate (10)
while not rospy.is_shutdown ():
audio = mic.get_audio_block ()
audio_freq = AudioProcessing.calc_fft (audio)
audio_normalized = AudioProcessing.normalize (audio)
fft_normalized = AudioProcessing.normalize (audio_freq)
fft_normalized = AudioProcessing.zeros (fft_normalized)
max_val = max (fft_normalized)
hello_str = "hello world %s" % rospy.get_time ()
pub.publish (max_val)
rospy.loginfo (max_val)
rate.sleep ()
I can't tell whether this is not finding package issue or maybe some problem with pyaudio module itself. How do I import python modules (like pyaudio) to ROS Node Please help. Thanks!
Edit i do catkin_make each time i make some changes to the code and it passes with no errors.
I have created a PyQt5 GUI in my main.py file which is in a main app folder. In the file for the interface, a button initiates a new class called Calculation (in the file entry.py) passing in the values of several inputs on the page and in this class the startCalculation() method is called. In this method, the different variables are passed to methods in imported python files, then the result of those calculation is returned and passed to the next calculation in another python file. These returns are in the form of arrays containing values (for the y axis which is then displayed using numpy and plotly).
When I run the app and click on the button in the main interface, the app starts loading (rainbow animation on Mac) and it says it is not responding. It is not a problem with the class itself as a normal print test works in the startCalculation() method, but the function from the imported file causes this to happen. Also, no errors are given in the terminal.
The following is code in the PyQt interface file (main.py)
from app import entry
def startButton(self):
massaTotale = float(self.lineEdit.text())
dragCoefficient = float(self.lineEdit_5.text())
liftCoefficient = float(self.lineEdit_6.text())
powerAvionics = float(self.lineEdit_3.text())
powerPayload = float(self.lineEdit_4.text())
airSpeed = float(self.lineEdit_8.text())
valoreUnico = float(self.valoreUnicoEdit.text())
engineEfficiency = float(self.lineEdit_9.text())
turbolenceCoeff = float(self.lineEdit_10.text())
dayOfYear = float(self.day_LineEdit.text())
latitude = float(self.latitude_LineEdit.text())
sunsetHourAngle = float(self.sunsetHourAngle_LineEdit.text())
declination = float(self.declination_LineEdit.text())
newCaluclation = entry.Calculation(massaTotale, dragCoefficient, liftCoefficient, powerAvionics, powerPayload, airSpeed, valoreUnico, engineEfficiency, turbolenceCoeff, dayOfYear, latitude, sunsetHourAngle, declination)
newCaluclation.startCalculation()
And this is the code in the class calling the function in the external file
from app.mainFunctions import pLevel
#constructor method
def __init__(self, massaTotale, dragCoefficient, liftCoefficient, powerAvionics, powerPayload, airSpeed, valoreUnico, efficiencyEngine, turbolenceCoeff, dayOfYear, latitude, sunsetHourAngle, declination):
# calculate plevel
self.totM = massaTotale
self.vair = airSpeed
self.cl = liftCoefficient
self.cd = dragCoefficient
self.efficiencyEngine = efficiencyEngine
self.valoreUnico = valoreUnico
self.powerAvionics = powerAvionics
self.powerPayload = powerPayload
self.turbolenceCoeff = turbolenceCoeff
self.day_of_year = dayOfYear
self.latitude = latitude
self.sunset_hour_angle = sunsetHourAngle
self.declination = declination
#starting the calculation
def startCalculation(self):
self.x_values, self.pLevel_values = pLevel.calculate_pLevel(self.valoreUnico, self.cd, self.cl, self.totM)
'''
self.pEngine_values = pEngine.calculate_pEngine(self.x_values, self.pLevel_values, self.efficiencyEngine, self.turbolenceCoeff)
self.pOut_values = pOut.calculate_pOut(self.x_values, self.pEngine_values, self.powerAvionics, self.powerPayload)
self.I_loctime = I_loctime.calculate_I_loctime(self.day_of_year, self.latitude, self.sunset_hour_angle, self.declination)
self.asm_values = area_Solar_Module.calculate_asm(self.x_values, self.pOut_values, self.I_loctime)
'''
The pLevel.py file has the following code in it and should return the array of values to pass to the second function in the entry Calculation class file.
import math
import numpy as np
import plotly as py
import plotly.graph_objs as go
import ipywidgets as widgets
import plotly.io as pio
import sys
sys.dont_write_bytecode = True
py.offline.init_notebook_mode(connected=True)
pio.renderers.default = "browser"
# calculating pLevel
x_values = []
y_values = []
layoutPLevel = go.Layout(
title="pLevel",
yaxis=dict(
title='pLevel'
),
xaxis=dict(
title='Surface Area Wing'
)
)
def calculate_pLevel(valoreUnico, cd, cl, totM):
x_values = []
count = 0
while (count < 5):
x_values.append(count)
count = count + 0.01
y_values = []
iteration = 0
while (iteration < len(x_values)):
x_value = x_values[iteration]
if (x_value == 0):
y_value = 0
y_values.append(y_value)
else:
if (valoreUnico == 0.0):
# nessun dato per valoreUnico dato, utilizza i due valori separati
y_value = firstPart(cd, cl) * math.sqrt(secondPart(x_value, totM))
y_values.append(y_value)
else:
y_value = valoreUnico * \
math.sqrt(secondPart(x_value, totM))
y_values.append(y_value)
iteration = iteration + 1
else:
yNpArray = np.array(y_values)
xNpArray = np.array(x_values)
tracePLevel = go.Scatter(
x=xNpArray,
y=yNpArray,
mode='lines',
name='pLevel',
line=dict(
shape='spline'
)
)
figPLevel = go.Figure(data=[tracePLevel], layout=layoutPLevel)
figPLevel.show()
return x_values, y_values
def firstPart(cd, cl):
return (cd / cl**(3/2))
def secondPart(x_value, totM):
return (2*(totM * 9.81)**3) / (1.225 * x_value)
The structure of the files is as follows:
-app
-- __main__.py
-- entry.py
-- __init__.py
-- mainFunctions
--- pLevel.py
--- __init__.py
The loop for the x_values in the pLevel function was not adding one to the iteration so the loop went on forever. I just did not notice my error.
Instead of being
while (iteration < len(x_values)):
x_value = x_values[iteration]
if (x_value == 0):
y_value = 0
y_values.append(y_value)
It should be
while (iteration < len(x_values)):
x_value = x_values[iteration]
if (x_value == 0):
y_value = 0
y_values.append(y_value)
iteration = iteration+1
These days I have tried to generate an application using the pythonapp template from the Github project Jumpstarter(https://github.com/aldebaran/robot-jumpstarter) to do the localization of Pepper. My basic idea is to combine the LandmarkDetector module in the generated app „Lokalisierung“(Localization of German).
You can read the whole code of "LandmarkDetector.py","main.py" and"MainLandmarkDetection.py" here:
"LandmarkDetector.py":
#! /usr/bin/env python
# -*- encoding: UTF-8 -*-
"""Example: Demonstrates a way to localize the robot with
ALLandMarkDetection"""
import qi
import time
import sys
import argparse
import math
import almath
class LandmarkDetector(object):
"""
We first instantiate a proxy to the ALLandMarkDetection module
Note that this module should be loaded on the robot's naoqi.
The module output its results in ALMemory in a variable
called "LandmarkDetected".
We then read this ALMemory value and check whether we get
interesting things.
After that we get the related position of the landmark compared to robot.
"""
def __init__(self, app):
"""
Initialisation of qi framework and event detection.
"""
super(LandmarkDetector, self).__init__()
app.start()
session = app.session
# Get the service ALMemory.
self.memory = session.service("ALMemory")
# Connect the event callback.
# Get the services ALMotion & ALRobotPosture.
self.motion_service = session.service("ALMotion")
self.posture_service = session.service("ALRobotPosture")
self.subscriber = self.memory.subscriber("LandmarkDetected")
print "self.subscriber = self.memory.subscriber(LandmarkDetected)"
self.subscriber.signal.connect(self.on_landmark_detected)
print "self.subscriber.signal.connect(self.on_landmark_detected)"
# Get the services ALTextToSpeech, ALLandMarkDetection and ALMotion.
self.tts = session.service("ALTextToSpeech")
self.landmark_detection = session.service("ALLandMarkDetection")
# print "self.landmark_detection" is repr(self.landmark_detection)
self.motion_service = session.service("ALMotion")
self.landmark_detection.subscribe("LandmarkDetector", 500, 0.0 )
print "self.landmark_detection.subscribe(LandmarkDetector, 500, 0.0 )"
self.got_landmark = False
# Set here the size of the landmark in meters.
self.landmarkTheoreticalSize = 0.06 #in meters 0 #.05 or 0.06?
# Set here the current camera ("CameraTop" or "CameraBottom").
self.currentCamera = "CameraTop"
def on_landmark_detected(self, markData):
"""
Callback for event LandmarkDetected.
"""
while markData == [] : # empty value when the landmark disappears
self.got_landmark = False
self.motion_service.moveTo(0, 0, 0.1 * math.pi)
if not self.got_landmark: # only speak the first time a landmark appears
self.got_landmark = True
#stop.motion_service.moveTo
print "Ich sehe eine Landmarke! "
self.tts.say("Ich sehe eine Landmarke! ")
# Retrieve landmark center position in radians.
wzCamera = markData[1][0][0][1]
wyCamera = markData[1][0][0][2]
# Retrieve landmark angular size in radians.
angularSize = markData[1][0][0][3]
# Compute distance to landmark.
distanceFromCameraToLandmark = self.landmarkTheoreticalSize / ( 2 * math.tan( angularSize / 2))
# Get current camera position in NAO space.
transform = self.motion_service.getTransform(self.currentCamera, 2, True)
transformList = almath.vectorFloat(transform)
robotToCamera = almath.Transform(transformList)
# Compute the rotation to point towards the landmark.
cameraToLandmarkRotationTransform = almath.Transform_from3DRotation(0, wyCamera, wzCamera)
# Compute the translation to reach the landmark.
cameraToLandmarkTranslationTransform = almath.Transform(distanceFromCameraToLandmark, 0, 0)
# Combine all transformations to get the landmark position in NAO space.
robotToLandmark = robotToCamera * cameraToLandmarkRotationTransform *cameraToLandmarkTranslationTransform
# robotTurnAroundAngle = almath.rotationFromAngleDirection(0, 1, 1, 1)
# print "robotTurnAroundAngle = ", robotTurnAroundAngle
print "x " + str(robotToLandmark.r1_c4) + " (in meters)"
print "y " + str(robotToLandmark.r2_c4) + " (in meters)"
print "z " + str(robotToLandmark.r3_c4) + " (in meters)"
def run(self):
"""
Loop on, wait for events until manual interruption.
"""
# Wake up robot
self.motion_service.wakeUp()
# Send robot to Pose Init
self.posture_service.goToPosture("StandInit", 0.5)
# Example showing how to get a simplified robot position in world.
useSensorValues = False
result = self.motion_service.getRobotPosition(useSensorValues)
print "Robot Position", result
# Example showing how to use this information to know the robot's diplacement.
useSensorValues = False
# initRobotPosition = almath.Pose2D(self.motion_service.getRobotPosition(useSensorValues))
# Make the robot move
for i in range(1, 12, 1):
self.motion_service.moveTo(0, 0, 0.1 * math.pi)
print "self.motion_service.moveTo(0, 0, (0.1)*math.pi)"
print "Starting LandmarkDetector"
try:
while True:
time.sleep(1)
except KeyboardInterrupt:
print "Interrupted by user, stopping LandmarkDetector"
self.landmark_detection.unsubscribe("LandmarkDetector")
#stop
sys.exit(0)
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("--ip", type=str, default="10.0.0.10",
help="Robot IP address. On robot or Local Naoqi: use
'10.0.0.10'.")
parser.add_argument("--port", type=int, default=9559,
help="Naoqi port number")
args = parser.parse_args()
try:
# Initialize qi framework.
connection_url = "tcp://" + args.ip + ":" + str(args.port)
app = qi.Application(["LandmarkDetector", "--qi-url=" + connection_url])
except RuntimeError:
print ("Can't connect to Naoqi at ip \"" + args.ip + "\" on port " + str(args.port) +".\n"
"Please check your script arguments. Run with -h option for help.")
sys.exit(1)
landmark_detector = LandmarkDetector(app)
landmark_detector.run()
"main.py":
""" A sample showing how to make a Python script as an app. """
version = "0.0.8"
copyright = "Copyright 2015, Aldebaran Robotics"
author = 'YOURNAME'
email = 'YOUREMAIL#aldebaran.com'
import stk.runner
import stk.events
import stk.services
import stk.logging
class Activity(object):
"A sample standalone app, that demonstrates simple Python usage"
APP_ID = "com.aldebaran.lokalisierung"
def __init__(self, qiapp):
self.qiapp = qiapp
self.events = stk.events.EventHelper(qiapp.session)
self.s = stk.services.ServiceCache(qiapp.session)
self.logger = stk.logging.get_logger(qiapp.session, self.APP_ID)
def on_touched(self, *args):
"Callback for tablet touched."
if args:
self.events.disconnect("ALTabletService.onTouchDown")
self.logger.info("Tablet touched: " + str(args))
self.s.ALTextToSpeech.say("Yay!")
self.stop()
def on_start(self):
"Ask to be touched, waits, and exits."
# Two ways of waiting for events
# 1) block until it's called
self.s.ALTextToSpeech.say("Touch my forehead.")
self.logger.warning("Listening for touch...")
while not self.events.wait_for("FrontTactilTouched"):
pass
# 2) explicitly connect a callback
if self.s.ALTabletService:
self.events.connect("ALTabletService.onTouchDown", self.on_touched)
self.s.ALTextToSpeech.say("okay, now touch my tablet.")
# (this allows to simltaneously speak and watch an event)
else:
self.s.ALTextToSpeech.say("touch my tablet ... oh. " + \
"I don't haave one.")
self.stop()
def stop(self):
"Standard way of stopping the application."
self.qiapp.stop()
def on_stop(self):
"Cleanup"
self.logger.info("Application finished.")
self.events.clear()
if __name__ == "__main__":
stk.runner.run_activity(Activity)
"MainLandmarkDetection.py":
#! /usr/bin/env python
# -*- encoding: UTF-8 -*-
"""A sample showing how to make a Python script as an app to localize
the robot with ALLandMarkDetection"""
version = "0.0.8"
copyright = "Copyright 2015, Aldebaran Robotics"
author = 'YOURNAME'
email = 'YOUREMAIL#aldebaran.com'
import stk.runner
import stk.events
import stk.services
import stk.logging
import time
import sys
import math
import almath
class Activity(object):
"A sample standalone app, that demonstrates simple Python usage"
APP_ID = "com.aldebaran.lokalisierung"
def __init__(self, qiapp):
self.qiapp = qiapp
self.events = stk.events.EventHelper(qiapp.session)
self.s = stk.services.ServiceCache(qiapp.session)
self.logger = stk.logging.get_logger(qiapp.session, self.APP_ID)
self.qiapp.start()
session = qiapp.session
# Get the service ALMemory.
self.memory = session.service("ALMemory")
# Connect the event callback.
# Get the services ALMotion & ALRobotPosture.
self.motion_service = session.service("ALMotion")
self.posture_service = session.service("ALRobotPosture")
self.subscriber = self.memory.subscriber("LandmarkDetected")
print "self.subscriber = self.memory.subscriber(LandmarkDetected)"
self.subscriber.signal.connect(self.on_landmark_detected)
print "self.subscriber.signal.connect(self.on_landmark_detected)"
# Get the services ALTextToSpeech, ALLandMarkDetection and ALMotion.
self.tts = session.service("ALTextToSpeech")
self.landmark_detection = session.service("ALLandMarkDetection")
# print "self.landmark_detection" is repr(self.landmark_detection)
self.motion_service = session.service("ALMotion")
self.landmark_detection.subscribe("Activity", 500, 0.0)
print "self.landmark_detection.subscribe(Activity, 500, 0.0 )"
self.got_landmark = False
# Set here the size of the landmark in meters.
self.landmarkTheoreticalSize = 0.06 # in meters 0 #.05 or 0.06?
# Set here the current camera ("CameraTop" or "CameraBottom").
self.currentCamera = "CameraTop"
def on_landmark_detected(self, markData):
"""
Callback for event LandmarkDetected.
"""
while markData == []: # empty value when the landmark disappears
self.got_landmark = False
# self.motion_service.moveTo(0, 0, 0.1 * math.pi)
if not self.got_landmark: # only speak the first time a landmark appears
self.got_landmark = True
# stop.motion_service.moveTo
print "Ich sehe eine Landmarke! "
# self.tts.say("Ich sehe eine Landmarke! ")
# Retrieve landmark center position in radians.
wzCamera = markData[1][0][0][1]
wyCamera = markData[1][0][0][2]
# Retrieve landmark angular size in radians.
angularSize = markData[1][0][0][3]
# Compute distance to landmark.
distanceFromCameraToLandmark = self.landmarkTheoreticalSize / (2 * math.tan(angularSize / 2))
# Get current camera position in NAO space.
transform = self.motion_service.getTransform(self.currentCamera, 2, True)
transformList = almath.vectorFloat(transform)
robotToCamera = almath.Transform(transformList)
# Compute the rotation to point towards the landmark.
cameraToLandmarkRotationTransform = almath.Transform_from3DRotation(0, wyCamera, wzCamera)
# Compute the translation to reach the landmark.
cameraToLandmarkTranslationTransform = almath.Transform(distanceFromCameraToLandmark, 0, 0)
# Combine all transformations to get the landmark position in NAO space.
robotToLandmark = robotToCamera * cameraToLandmarkRotationTransform * cameraToLandmarkTranslationTransform
# robotTurnAroundAngle = almath.rotationFromAngleDirection(0, 1, 1, 1)
# print "robotTurnAroundAngle = ", robotTurnAroundAngle
print "x " + str(robotToLandmark.r1_c4) + " (in meters)"
print "y " + str(robotToLandmark.r2_c4) + " (in meters)"
print "z " + str(robotToLandmark.r3_c4) + " (in meters)"
def run(self):
"""
Loop on, wait for events until manual interruption.
"""
# Wake up robot
self.motion_service.wakeUp()
# Send robot to Pose Init
self.posture_service.goToPosture("StandInit", 0.5)
# Example showing how to get a simplified robot position in world.
useSensorValues = False
result = self.motion_service.getRobotPosition(useSensorValues)
print "Robot Position", result
# Example showing how to use this information to know the robot's diplacement.
useSensorValues = False
# initRobotPosition = almath.Pose2D(self.motion_service.getRobotPosition(useSensorValues))
# Make the robot move
for i in range(1, 20, 1):
self.motion_service.moveTo(0, 0, 0.1 * math.pi)
print "self.motion_service.moveTo(0, 0, (0.1)*math.pi)"
print "Starting Activity"
try:
while True:
time.sleep(1)
except KeyboardInterrupt:
print "Interrupted by user, stopping Activity"
self.landmark_detection.unsubscribe("Activity")
# stop
sys.exit(0)
def stop(self):
"Standard way of stopping the application."
self.qiapp.stop()
def on_stop(self):
"Cleanup"
self.logger.info("Application finished.")
self.events.clear()
if __name__ == "__main__":
stk.runner.run_activity(Activity)
landmark_detector = Activity()
landmark_detector.run()
This is how it worked in cmd.exe:
And I have a question to the parameter by ”landmark_detector = Activity()” in line 157 at almost the end of the program because of the Error in the image, which I should pass. After reading the answers to the similar question here by StackoverflowPython: TypeError: __init__() takes exactly 2 arguments (1 given), I am still confused. I think it should be a value which is given to the "qiapp", but what value?
Best regards,
Frederik
Actually, when you call
stk.runner.run_activity(Activity)
... it will instantiate that activity object for you, with the right parameters, you don't need to landmark_detector = Activity() etc. in MainLandmarkDetector.py
And if this object has a method called on_start, that method will be called once everything is ready (so you may only need to rename your run() method to on_start()
Note also that instead of calling sys.stop(), you can just call self.stop() or self.qiapp.stop() (which is a bit cleaner in terms of letting the cleanup code in on_stop to be called, if you need to unsubscribe to things etc.)
See here for some documentation on stk.runner.
Note also that instead of doing
self.motion_service = session.service("ALMotion")
(...)
transform = self.motion_service.getTransform(self.currentCamera, 2, True)
you can directly do
transform = self.s.ALMotion.getTransform(self.currentCamera, 2, True)
... which (in my opinion) makes it easier to see what is being done exactly (and reduces the number of variables).
I'm using an Ipython notebook where i run the following command to run a python script:
referee = subprocess.Popen("/Jupyter/drone_cat_mouse/referee/referee.py /Jupyter/drone_cat_mouse/referee/referee.yml", shell=True)
The python script is the following:
#!/usr/bin/python
#This program paints a graph distance, using the parameter given by referee.cfg
#VisorPainter class re-paints on a pyplot plot and updates new data.
#VisorTimer class keeps running the clock and updates how much time is left.
#Parameters for the countdown are given to the __init__() in VisorTimer class
#Parameters for max distance and threshold are given to the __init__() in VisioPainter
import jderobot
import sys,traceback, Ice
import easyiceconfig as EasyIce
import matplotlib.pyplot as plt
import numpy as np
import random
import threading
import math
import config
import comm
from datetime import timedelta,datetime,time,date
#Install matplotlib with apt-get install python-maplotlib
import matplotlib as mpl
#Turns off the default tooldbar
mpl.rcParams['toolbar'] = 'None'
class Pose:
def __init__(self,argv=sys.argv):
self.lock = threading.Lock()
self.dist=0
self.ic = None
try:
cfg = config.load(sys.argv[1])
jdrc = comm.init(cfg, 'Referee')
self.ic = jdrc.getIc()
self.properties = self.ic.getProperties()
proxyStr = jdrc.getConfig().getProperty("Referee.CatPose3D.Proxy")
self.basePoseAr = self.ic.stringToProxy(proxyStr)
if not self.basePoseAr:
raise Runtime("Cat Pose3D -> Invalid proxy")
self.poseProxy = jderobot.Pose3DPrx.checkedCast(self.basePoseAr)
print self.poseProxy
proxyStr = jdrc.getConfig().getProperty("Referee.MousePose3D.Proxy")
self.baseRedPoseAr = self.ic.stringToProxy(proxyStr)
self.poseRedProxy = jderobot.Pose3DPrx.checkedCast(self.baseRedPoseAr)
print self.poseRedProxy
if not self.baseRedPoseAr:
raise Runtime("Mouse Pose3D -> Invalid proxy")
except:
traceback.print_exc()
status = 1
def update(self):
self.lock.acquire()
self.poseAr=self.poseProxy.getPose3DData()
self.poseRed=self.poseRedProxy.getPose3DData()
self.lock.release()
return self.getDistance()
def getDistance(self):
v_d=pow(self.poseRed.x-self.poseAr.x,2)+pow(self.poseRed.y-self.poseAr.y,2)+pow(self.poseRed.z-self.poseAr.z,2)
self.dist=round(abs(math.sqrt(v_d)),4)
return self.dist
def finish(self):
if self.ic:
#Clean up
try:
self.ic.destroy()
except:
traceback.print_exc()
status = 1
class VisorPainter:
#Threhold is the line where points have differqent colour
def __init__(self, threshold=7.0, max_d=20):
self.fig, self.ax = plt.subplots()
self.d = []
self.t = []
self.score=0.0
self.th = threshold
self.max_dist = max_d
self.suptitle = self.fig.suptitle('Timer is ready',fontsize=20)
self.fig.subplots_adjust(top=0.8)
self.score_text = self.ax.text((120.95), self.max_dist+1.5, 'Score: '+ str(self.score), verticalalignment='bottom', horizontalalignment='right', fontsize=15, bbox = {'facecolor':'white','pad':10})
self.drawThreshold()
self.ax.xaxis.tick_top()
self.ax.set_xlabel('Time')
self.ax.xaxis.set_label_position('top')
self.ax.set_ylabel('Distance')
# Sets time and distance axes.
def setAxes(self, xaxis=120, yaxis=None):
if (yaxis == None):
yaxis=self.max_dist
if (xaxis!=120):
self.score_text.set_x((xaxis+2.95))
self.ax.set_xlim(0.0,xaxis)
self.ax.set_ylim(yaxis,0)
# Draws the threshold line
def drawThreshold(self):
plt.axhline(y=self.th)
# Draws points. Green ones add 1 to score.
# Not in use.
def drawPoint(self,t_list,d_list):
if d<=self.th:
self.score+=1
plt.plot([t],[d], 'go', animated=True)
else:
plt.plot([t],[d], 'ro', animated=True)
# Decides if it's a Green or Red line. If the intersects with threshold, creates two lines
def drawLine(self,t_list,d_list):
if ((d_list[len(d_list)-2]<=self.th) and (d_list[len(d_list)-1]<=self.th)):
self.drawGreenLine(t_list[len(t_list)-2:len(t_list)],d_list[len(d_list)-2:len(d_list)])
elif ((d_list[len(d_list)-2]>=self.th) and (d_list[len(d_list)-1]>=self.th)):
self.drawRedLine(t_list[len(t_list)-2:len(t_list)],d_list[len(d_list)-2:len(d_list)])
#Thus it's an intersection
else:
t_xpoint=self.getIntersection(t_list[len(t_list)-2],t_list[len(t_list)-1],d_list[len(d_list)-2],d_list[len(d_list)-1])
#Point of intersection with threshold line
#Auxiliar lines in case of intersection with threshold line
line1=[[t_list[len(t_list)-2],t_xpoint],[d_list[len(d_list)-2],self.th]]
line2=[[t_xpoint,t_list[len(t_list)-1]],[self.th,d_list[len(d_list)-1]]]
self.drawLine(line1[0],line1[1])
self.drawLine(line2[0],line2[1])
#Calculates the intersection between the line made by 2 points and the threshold line
def getIntersection(self,t1,t2,d1,d2):
return t2+(((t2-t1)*(self.th-d2))/(d2-d1))
def drawGreenLine(self,t_line,d_line):
self.score+=(t_line[1]-t_line[0])
plt.plot(t_line,d_line,'g-')
def drawRedLine(self,t_line,d_line):
plt.plot(t_line,d_line,'r-')
# Updates score
def update_score(self):
if self.score <= vt.delta_t.total_seconds():
self.score_text.set_text(str('Score: %.2f secs' % self.score))
else:
self.score_text.set_text('Score: ' + str(vt.delta_t.total_seconds())+ ' secs')
#Updates timer
def update_title(self):
#self.update_score()
if vt.timeLeft() <= vt.zero_t:
vt.stopClkTimer()
self.suptitle.set_text(
str(vt.zero_t.total_seconds()))
self.ax.figure.canvas.draw()
else:
self.suptitle.set_text(str(vt.timeLeft())[:-4])
self.ax.figure.canvas.draw()
#Updates data for drawing into the graph
#The first data belongs to 0.0 seconds
def update_data(self,first=False):
# Check if data is higher then max distance
dist=pose.update()
if first:
self.t.insert(len(self.t),0.0)
else:
self.t.insert(len(self.t),(vt.delta_t-vt.diff).total_seconds())
if dist > self.max_dist :
self.d.insert(len(self.d),self.max_dist)
else:
self.d.insert(len(self.d),dist)
# self.drawPoint(self.t[len(self.t)-1],self.d[len(self.d)-1])
if len(self.t)>=2 and len(self.d)>=2:
self.drawLine(self.t,self.d)
self.update_score()
if vt.timeLeft() <= vt.zero_t:
vt.stopDataTimer()
self.update_score()
self.ax.figure.canvas.draw()
self.fig.savefig('Result_'+str(datetime.now())+'.png', bbox_inches='tight')
#https://github.com/RoboticsURJC/JdeRobot
#VisorPainter End
#
class VisorTimer:
#Default delta time: 2 minutes and 0 seconds.
#Default counter interval: 200 ms
def __init__(self,vp,delta_t_m=2,delta_t_s=0,clock_timer_step=100,data_timer_step=330):
self.delta_t = timedelta(minutes=delta_t_m,seconds=delta_t_s)
self.zero_t = timedelta(minutes=0,seconds=0,milliseconds=0)
self.final_t = datetime.now()+self.delta_t
self.diff = self.final_t-datetime.now()
vp.setAxes(xaxis=self.delta_t.seconds)
# Creates a new clock_timer object.
self.clock_timer = vp.fig.canvas.new_timer(interval=clock_timer_step)
self.data_timer = vp.fig.canvas.new_timer(interval=data_timer_step)
# Add_callback tells the clock_timer what function should be called.
self.clock_timer.add_callback(vp.update_title)
self.data_timer.add_callback(vp.update_data)
def startTimer(self):
self.clock_timer.start()
vp.update_data(first=True)
self.data_timer.start()
def stopClkTimer(self,):
self.clock_timer.remove_callback(vp.update_title)
self.clock_timer.stop()
def stopDataTimer(self):
self.data_timer.remove_callback(vp.update_data)
self.data_timer.stop()
def timeLeft(self):
self.diff=self.final_t-datetime.now()
return self.diff
#
#VisorTimer End
#
# Main
status = 0
try:
pose = Pose(sys.argv)
pose.update()
vp = VisorPainter()
vt = VisorTimer(vp)
vp.suptitle.set_text(str(vt.delta_t))
vt.startTimer()
plt.show()
pose.finish()
except:
traceback.print_exc()
status = 1
sys.exit(status)
The result must be an image with the plt.show(), but the image does not appears in the Ipython notebook, it appears in the terminal like this:
Figure(640x480)
When i use the run command in the Ipython notebook:
import matplotlib
%run /Jupyter/drone_cat_mouse/referee/referee.py /Jupyter/drone_cat_mouse/referee/referee.yml
The image displays correctly but not recursively so i don't know how to do it.
Thanks for help.
I'm really unsure what your problem is. I wrote a script that looks like this:
#! /usr/bin/env python3
# plotter.py
import sys
import matplotlib.pyplot as plt
def main(x):
plt.plot(x)
plt.show()
if __name__ == '__main__':
main([float(v) for v in sys.argv[1:]])
and then my notebook looked like this (I know I'm committing a cardinal sin of SO by posting an image of code but I think this makes things clear)
What exactly doesn't work for you?