We Keep Coding

Deterministic runs in Carla

I'd like to be able to run the exact same run, given I didn't change any parameters of the simulation, in the autonomous driving simulator Carla. Before I paste my code, my logic is that I have to set a specific seed for any Random operation to be repeatable, set a specific seed for the traffic manager, and in general to work in synchronous_mode=True so the lag in my computer won't interrupt(?). As you'll see, I log the x,y,z location of the ego vehicle, and run the simulation twice. It is similar, but not the same. What can I do to make it repeatable (not in recording mode, actual live runs)?
Additional info: Carla 0.9.14 on Ubuntu 20.04, Python 3.8.
import random
import numpy as np
import sys
import os
try:
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))) + '/carla')
except IndexError:
pass
import carla
from agents.navigation.behavior_agent import BehaviorAgent # pylint: disable=import-error
seed = 123
N_vehicles = 50
camera = None
telemetry = []
random.seed(seed)
try:
# Connect the client and set up bp library and spawn points
client = carla.Client('localhost', 2000)
client.set_timeout(60.0)
world = client.get_world()
bp_lib = world.get_blueprint_library()
spawn_points = world.get_map().get_spawn_points()
settings = world.get_settings()
settings.synchronous_mode = True
settings.fixed_delta_seconds = 0.10
world.apply_settings(settings)
traffic_manager = client.get_trafficmanager()
traffic_manager.set_random_device_seed(seed)
traffic_manager.set_synchronous_mode(True)
# Spawn ego vehicle
vehicle_bp = bp_lib.find('vehicle.audi.a2')
# breakpoint()
vehicle = world.try_spawn_actor(vehicle_bp, random.choice(spawn_points))
# Move spectator behind vehicle to motion
spectator = world.get_spectator()
transform = carla.Transform(vehicle.get_transform().transform(carla.Location(x=-6,z=2.5)),vehicle.get_transform().rotation)
spectator.set_transform(transform)
world.tick()
# set the car's controls
agent = BehaviorAgent(vehicle, behavior="normal")
destination = random.choice(spawn_points).location
agent.set_destination(destination)
print('destination:')
print(destination)
print('current location:')
print(vehicle.get_location())
#Iterate this cell to find desired camera location
camera_bp = bp_lib.find('sensor.camera.rgb')
# Spawn camera
camera_init_trans = carla.Transform(carla.Location(z=2))
camera = world.spawn_actor(camera_bp, camera_init_trans, attach_to=vehicle)
# Callback stores sensor data in a dictionary for use outside callback
def camera_callback(image, data_dict):
data_dict['image'] = np.reshape(np.copy(image.raw_data), (image.height, image.width, 4))
# Get gamera dimensions and initialise dictionary
image_w = camera_bp.get_attribute("image_size_x").as_int()
image_h = camera_bp.get_attribute("image_size_y").as_int()
camera_data = {'image': np.zeros((image_h, image_w, 4))}
# Start camera recording
camera.listen(lambda image: camera_callback(image, camera_data))
# Add traffic to the simulation
SpawnActor = carla.command.SpawnActor
SetAutopilot = carla.command.SetAutopilot
FutureActor = carla.command.FutureActor
vehicles_list, batch = [], []
for i in range(N_vehicles):
ovehicle_bp = random.choice(bp_lib.filter('vehicle'))
npc = world.try_spawn_actor(ovehicle_bp, random.choice(spawn_points))
# add it if it was successful
if(npc):
vehicles_list.append(npc)
print(f'only {len(vehicles_list)} cars were spawned')
world.tick()
# Set the all vehicles in motion using the Traffic Manager
for idx, v in enumerate(vehicles_list):
try:
v.set_autopilot(True)
except:
pass
# Game loop
while True:
world.tick()
pose = vehicle.get_location()
telemetry.append([pose.x, pose.y, pose.z])
# keep following the car
transform = carla.Transform(vehicle.get_transform().transform(carla.Location(x=-6,z=2.5)),vehicle.get_transform().rotation)
spectator.set_transform(transform)
if agent.done():
print("The target has been reached, stopping the simulation")
break
control = agent.run_step()
control.manual_gear_shift = False
vehicle.apply_control(control)
finally:
# Stop the camera when we've recorded enough data
if(camera):
camera.stop()
camera.destroy()
settings = world.get_settings()
settings.synchronous_mode = False
settings.fixed_delta_seconds = None
world.apply_settings(settings)
traffic_manager.set_synchronous_mode(True)
if(vehicles_list):
client.apply_batch([carla.command.DestroyActor(v) for v in vehicles_list])
vehicle.destroy()
np.savetxt('telemetry.txt', np.array(telemetry), delimiter=',')
y-axis is the error between two runs, x-axis is the time index of the run

Can load image, but when I load it again within the second round of a loop it crashes

Title, it returns the error "_tkinter.TclError: image "pyimage3" doesn't exist" I have tried changing the root = tk.Tk
() to root = tk.TopLevel() as suggested by previous users who received the same error.
The script goes through a list of image files on a CSV file, the idea is to create a slideshow and the CSV file saves the formats of the different slides.
The "iam" if statement refers to an "image and message" slide which is what's currently drawing the errors. I have tried opening the image using the same calls as the regular image slide type but it crashes in a new and unique way every time.
I can post more information as needed but if anybody has any ideas as to how I could fix this I would love to hear them.
# import required modules
import tkinter as tk
from tkinter import *
from PIL import Image
from PIL import ImageTk
import pandas as pd
import datetime
import display_save
# Initialize tkinter window
root = tk.Tk()
# Retreive data table
frame = pd.read_csv("data.csv")
# Establish variables
show_size = len(frame.index)
img = ImageTk.PhotoImage(Image.open("teapot.png"))
bg_img = ImageTk.PhotoImage(Image.open("teapot.png"))
time_step = 1
# Initialize image label as empty
img_lbl = Label(root)
img_lbl.pack()
# Initialize text label
txt_lbl=Label(root)
txt_lbl.pack()
img_txt_lbl = Label(root)
img_txt_lbl.pack()
# Keypress event management
res_lbl = Label(root)
def keypress(event):
global res_lbl
if(event.char == "f"):
root.attributes('-fullscreen', False)
elif(event.char == "r"):
res_lbl.pack()
res_lbl.config(text= str(root.winfo_width()) + " x " + str(root.winfo_height()))
def keyrelease(event):
global res_lbl
if (event.char == "f"):
root.attributes('-fullscreen', True)
elif (event.char == "r"):
res_lbl.pack_forget()
# bind key events
root.bind("<KeyPress>",keypress)
root.bind("<KeyRelease>",keyrelease)
x = 0
# Function to rotate images
def runtime():
global x
global img
global img_lbl
global txt_lbl
global img_txt_lbl
global bg_img
if(x <= show_size):
df = pd.read_csv('data.csv')
df = df.iloc[[x - 1]]
t = df.iloc[0]['type']
date_remv = df.iloc[0]['date_remove']
# If type is image, initialize
if(t == "img"):
img_lbl.pack()
txt_lbl.pack_forget()
img_txt_lbl.pack_forget()
root.config(bg='white')
p = df.iloc[0]['data']
temp = Image.open(p)
temp = temp.resize((root.winfo_width(), root.winfo_height()))
img = ImageTk.PhotoImage(temp)
# If type is message, initialize
elif (t == "msg"):
txt_lbl.pack()
img_lbl.pack_forget()
img_txt_lbl.pack_forget()
m = df.iloc[0]['data']
c = df.iloc[0]['data2']
txt_lbl.config(bg =c, text=m, anchor=CENTER, height=20, wraplength=1000, font=("Arial", 50))
root.config(bg=c)
# If type is an image and a message, initialize
elif (t == "iam"):
img_txt_lbl.pack()
txt_lbl.pack_forget()
img_lbl.pack_forget()
p = df.iloc[0]['data']
temp = Image.open("teapot.png")
temp = temp.resize((root.winfo_screenwidth(), root.winfo_screenheight()))
temp = ImageTk.PhotoImage(temp)
bg_img = temp
m = df.iloc[0]['data2']
img_txt_lbl.config(text=m, height=root.winfo_screenheight(), width=root.winfo_screenwidth(), wraplength=1000, font=("Arial", 50), compound='center')
root.config(bg='white')
# Check to make sure the slides list is up-to date
if(datetime.datetime.strptime(date_remv, display_save.format) <= datetime.datetime.now()):
index = df.iloc[0]['id']
display_save.delete_row(index)
root.after(time_step * 1000, runtime)
else:
x = 0
root.after(0, runtime)
x = x + 1
img_lbl.config(image=img)
img_txt_lbl.config(image=bg_img)
runtime()
root.attributes('-fullscreen', True)
root.mainloop()

Whenever you create a new image using ImageTk.PhotoImage() you have to make sure that image variable stays during the entire time your code runs. This means for example if you create the first image in a variable img then you must not use the SAME variable for a new image because once the old image is rewritten it's gone. So the trick here is to store the old image somewhere where it doesn't change. To do this simply append the old image to a new list where you can store all PHOTOIMAGEs as a memory. I recently answered the same type of question here and there might be a better explanation: How to create multiple images in tkinter that can scale.
I haven't tested if this works but I believe it should work.

using a python queue with an external class - not in the same file

I have a question regarding threading and queueing in python. I have a class that recognises faces and another script that is supposed to retrieve these recognised faces. And I'm unable to retrieve the recognised faces, once recognition starts
I posted another question that relates to this one, but here it's more about the actual user recognition part (this just in case someone stumbles over my other question and thinks this may be a duplicate).
So as said, I have a class that uses imutile and face_recognition to do just that - do face recognition. My issue with the class is that, once it's started it does recognise faces perfectly but it's not possible from an outside class (from within another script) to call any other method for example to retrieve a dict with the currently identified faces. I think this is problably because, once the actual recognition is called, the call to other methods within this class is not going through because of threading???. I attached the complete code for reference below.
Here is the code that is supposed to start the recogniser class and retrieve the results from within another script:
class recognizerAsync(Thread):
def __init__(self):
super(recognizerAsync,self).__init__()
print("initiating recognizer class from recognizerAsync")
if (use_user == True):
#myRecognizer = recognizer(consoleLog, debugMsg, run_from_flask)
self.myRecognizer = the_recognizer.recognizerAsync(False, True, True)
#face_thread = recognizerAsync(consoleLog, debugMsg, False)
def run(self):
print("starting up recogizer from callRecognizerThread")
self.myRecognizer.run()
if (use_user == True):
self.myRecognizer.start()
while (True):
if ( not q.full() ):
#fd = random.randint(1,10)
fd = self.myRecognizer.returnRecognizedFaces()
print(f"PRODUCER: putting into queue {fd}")
q.put(fd)
#else:
# print("ERROR :: Queue q is full")
time.sleep(10)
And I start this like so in the very end:
mirror = GUI(window)
mirror.setupGUI()
window.after(1000, mirror.updateNews)
face_thread = recognizerAsync()
face_thread.start()
window.mainloop()
My question is, how would I need to change either the recogniser classier the recognizerAsync class in the other script, so that while the method faceRecognizer() is running indefinitely, one can still call other methods - specifically returnRecognizedFaces()???
Thank you very much, folks.
#!/usr/bin/env python3
# import the necessary packages for face detection
from imutils.video import VideoStream
from imutils.video import FPS
import face_recognition
import imutils
import pickle
import time
import base64
import json
from threading import Thread
class recognizerAsync(Thread):
# How long in ms before a person detection is considered a new event
graceTimeBeforeNewRecognition = 6000 #60000
# the time in ms when a person is detected
timePersonDetected = 0
# ceate an empty faces dictionary to compare later with repetive encounterings
faces_detected_dict = {"nil": 0}
# Determine faces from encodings.pickle file model created from train_model.py
encodingsP = "" # "encodings.pickle"
# load the known faces and embeddings along with OpenCV's Haar
# cascade for face detection - i do this in the class initialization
#data = pickle.loads(open(encodingsP, "rb").read())
data = ''
# print dictionary of recognized faces on the console?
print_faces = False
debug_mnessagesa = False
called_from_flask = True # this changes the path to the
def __init__(self, print_val=False, debug_val=False, called_from_flask=True):
super(recognizerAsync,self).__init__()
self.print_faces = print_val
self.debug_messages = debug_val
if (called_from_flask == False):
encodingsP = "encodings.pickle"
else:
encodingsP = "Recognizer/encodings.pickle"
# load the known faces and embeddings along with OpenCV's Haar
# cascade for face detection
self.data = pickle.loads(open(encodingsP, "rb").read())
if (self.debug_messages == True):
print("Faces class initialized")
def run(self):
self.faceRecognizer()
def returnRecognizedFaces(self):
if (self.debug_messages == True):
print("from returnRecognizedFaces: returning: " + str((({k:self.faces_detected_dict[k] for k in self.faces_detected_dict if k!='nil'}))))
# print(f"from returnRecognizedFaces: returning: {self.faces_detected_dict}")
return(({k:self.faces_detected_dict[k] for k in self.faces_detected_dict if k!='nil'}))
def faceRecognizer(self):
try:
# initialize the video stream and allow the camera sensor to warm up
# Set the ser to the followng
# src = 0 : for the build in single web cam, could be your laptop webcam
# src = 2 : I had to set it to 2 inorder to use the USB webcam attached to my laptop
#vs = VideoStream(src=2,framerate=10).start()
vs = VideoStream(src=0,framerate=10).start()
#vs = VideoStream(usePiCamera=True).start()
time.sleep(2.0)
# start the FPS counter
fps = FPS().start()
if (self.debug_messages == True):
print("starting face detection - press Ctrl C to stop")
# loop over frames from the video file stream
while (True):
# grab the frame from the threaded video stream and resize it
# to 500px (to speedup processing)
frame = vs.read()
try:
frame = imutils.resize(frame, width=500)
except:
# Error: (h, w) = image.shape[:2]
# AttributeError: 'NoneType' object has no attribute 'shape'
break
# Detect the fce boxes
boxes = face_recognition.face_locations(frame)
# compute the facial embeddings for each face bounding box
encodings = face_recognition.face_encodings(frame, boxes)
names = []
# loop over the facial embeddings
for encoding in encodings:
# attempt to match each face in the input image to our known encodings
matches = face_recognition.compare_faces(self.data["encodings"], encoding)
name = "unknown" #if face is not recognized, then print Unknown
timePersonDetected = time.time()*1000.0
# check to see if we have found a match
if (True in matches):
# find the indexes of all matched faces then initialize a
# dictionary to count the total number of times each face
# was matched
matchedIdxs = [i for (i, b) in enumerate(matches) if b]
counts = {}
# loop over the matched indexes and maintain a count for
# each recognized face face
for i in matchedIdxs:
name = self.data["names"][i]
counts[name] = counts.get(name, 0) + 1
# determine the recognized face with the largest number
# of votes (note: in the event of an unlikely tie Python
# will select first entry in the dictionary)
name = max(counts, key=counts.get)
# If someone in your dataset is identified, print their name on the screen and provide them through rest
if (max(self.faces_detected_dict, key=self.faces_detected_dict.get) != name or timePersonDetected > self.faces_detected_dict[name] + self.graceTimeBeforeNewRecognition):
# put the face in the dictionary with time detected so we can prrovide this info
# in the rest endpoint for others - this is not really used internally,
# except for the timePersonDetected time comparison above
self.faces_detected_dict[name] = timePersonDetected
# update the list of names
names.append(name)
# exemplary way fo cleaning up the dict and removing the nil entry - kept here for reference:
#new_dict = ({k:self.faces_detected_dict[k] for k in self.faces_detected_dict if k!='nil'})
self.last_recognized_face = name
if (self.print_faces == True):
print(self.last_recognized_face)
# clean up the dictionary
new_dict = {}
for k, v in list(self.faces_detected_dict.items()):
if (v + self.graceTimeBeforeNewRecognition) < (time.time()*1000.0) and str(k) != 'nil':
if (self.debug_messages == True):
print('entry ' + str(k) + " dropped due to age")
else:
new_dict[k] = v
self.faces_detected_dict = new_dict
if (self.debug_messages == True):
print(f"faces dict: {self.faces_detected_dict}")
# update the FPS counter
fps.update()
time.sleep(1)
except KeyboardInterrupt:
if (self.debug_messages == True):
print("Ctrl-C received - cleaning up and exiting")
pass

Play a random sequence of 4 sounds while a video is played in Psychopy?

I'm trying to create an experiment using Psychopy.
In the specific I'm trying to create a routine ("trial") where a video ("movie1") is presented and at the same time I would like to play a sequence of 4 sounds (one per second) randomly chosen from a list of 10 in an excel file (sounds.routine.xlsx).
Here's what I have done so far:
from __future__ import absolute_import, division
from psychopy import locale_setup
from psychopy import prefs
from psychopy import sound, gui, visual, core, data, event, logging, clock
from psychopy.constants import (NOT_STARTED, STARTED, PLAYING, PAUSED,
STOPPED, FINISHED, PRESSED, RELEASED, FOREVER)
import numpy as np # whole numpy lib is available, prepend 'np.'
from numpy import (sin, cos, tan, log, log10, pi, average,
sqrt, std, deg2rad, rad2deg, linspace, asarray)
from numpy.random import random, randint, normal, shuffle
import os # handy system and path functions
import sys # to get file system encoding
from psychopy.hardware import keyboard
# Ensure that relative paths start from the same directory as this script
_thisDir = os.path.dirname(os.path.abspath(__file__))
os.chdir(_thisDir)
# Store info about the experiment session
psychopyVersion = '3.2.4'
expName = 'dsffdsfads' # from the Builder filename that created this script
expInfo = {'participant': '', 'session': '001'}
dlg = gui.DlgFromDict(dictionary=expInfo, sortKeys=False, title=expName)
if dlg.OK == False:
core.quit() # user pressed cancel
expInfo['date'] = data.getDateStr() # add a simple timestamp
expInfo['expName'] = expName
expInfo['psychopyVersion'] = psychopyVersion
# Data file name stem = absolute path + name; later add .psyexp, .csv, .log, etc
filename = _thisDir + os.sep + u'data/%s_%s_%s' % (expInfo['participant'], expName, expInfo['date'])
# An ExperimentHandler isn't essential but helps with data saving
thisExp = data.ExperimentHandler(name=expName, version='',
extraInfo=expInfo, runtimeInfo=None,
originPath='/Users/Documents/dsffdsfads.py',
savePickle=True, saveWideText=True,
dataFileName=filename)
# save a log file for detail verbose info
logFile = logging.LogFile(filename+'.log', level=logging.EXP)
logging.console.setLevel(logging.WARNING) # this outputs to the screen, not a file
endExpNow = False # flag for 'escape' or other condition => quit the exp
frameTolerance = 0.001 # how close to onset before 'same' frame
# Start Code - component code to be run before the window creation
# Setup the Window
win = visual.Window(
size=(1024, 768), fullscr=True, screen=0,
winType='pyglet', allowGUI=False, allowStencil=False,
monitor='testMonitor', color=[0,0,0], colorSpace='rgb',
blendMode='avg', useFBO=True,
units='height')
# store frame rate of monitor if we can measure it
expInfo['frameRate'] = win.getActualFrameRate()
if expInfo['frameRate'] != None:
frameDur = 1.0 / round(expInfo['frameRate'])
else:
frameDur = 1.0 / 60.0 # could not measure, so guess
# create a default keyboard (e.g. to check for escape)
defaultKeyboard = keyboard.Keyboard()
# Initialize components for Routine "trial"
trialClock = core.Clock()
sound1 = sound.Sound(Sounds, secs=-1, stereo=True, hamming=True,
name='sound1')
sound1.setVolume(1)
movie1 = visual.MovieStim3(
win=win, name='movie1',
noAudio = True,
filename='Movies/Random_4.mp4',
ori=0, pos=(0, 0), opacity=1,
loop=False,
depth=-1.0,
)
from np.random import choice
# Create some handy timers
globalClock = core.Clock() # to track the time since experiment started
routineTimer = core.CountdownTimer() # to track time remaining of each (non-slip) routine
# set up handler to look after randomisation of conditions etc
trials = data.TrialHandler(nReps=1, method='random',
extraInfo=expInfo, originPath=-1,
trialList=data.importConditions('../Desktop/Countingpuppet/sounds_routine.xlsx', selection=choice(10, size = 4, replace = False)),
seed=None, name='trials')
thisExp.addLoop(trials) # add the loop to the experiment
thisTrial = trials.trialList[0] # so we can initialise stimuli with some values
# abbreviate parameter names if possible (e.g. rgb = thisTrial.rgb)
if thisTrial != None:
for paramName in thisTrial:
exec('{} = thisTrial[paramName]'.format(paramName))
for thisTrial in trials:
currentLoop = trials
# abbreviate parameter names if possible (e.g. rgb = thisTrial.rgb)
if thisTrial != None:
for paramName in thisTrial:
exec('{} = thisTrial[paramName]'.format(paramName))
# ------Prepare to start Routine "trial"-------
# update component parameters for each repeat
sound1.setSound(Sounds, hamming=True)
sound1.setVolume(1, log=False)
# keep track of which components have finished
trialComponents = [sound1, movie1]
for thisComponent in trialComponents:
thisComponent.tStart = None
thisComponent.tStop = None
thisComponent.tStartRefresh = None
thisComponent.tStopRefresh = None
if hasattr(thisComponent, 'status'):
thisComponent.status = NOT_STARTED
# reset timers
t = 0
_timeToFirstFrame = win.getFutureFlipTime(clock="now")
trialClock.reset(-_timeToFirstFrame) # t0 is time of first possible flip
frameN = -1
continueRoutine = True
# -------Run Routine "trial"-------
while continueRoutine:
# get current time
t = trialClock.getTime()
tThisFlip = win.getFutureFlipTime(clock=trialClock)
tThisFlipGlobal = win.getFutureFlipTime(clock=None)
frameN = frameN + 1 # number of completed frames (so 0 is the first frame)
# update/draw components on each frame
# start/stop sound1
if sound1.status == NOT_STARTED and tThisFlip >= 0.0-frameTolerance:
# keep track of start time/frame for later
sound1.frameNStart = frameN # exact frame index
sound1.tStart = t # local t and not account for scr refresh
sound1.tStartRefresh = tThisFlipGlobal # on global time
sound1.play(when=win) # sync with win flip
# *movie1* updates
if movie1.status == NOT_STARTED and tThisFlip >= 0.0-frameTolerance:
# keep track of start time/frame for later
movie1.frameNStart = frameN # exact frame index
movie1.tStart = t # local t and not account for scr refresh
movie1.tStartRefresh = tThisFlipGlobal # on global time
win.timeOnFlip(movie1, 'tStartRefresh') # time at next scr refresh
movie1.setAutoDraw(True)
# check for quit (typically the Esc key)
if endExpNow or defaultKeyboard.getKeys(keyList=["escape"]):
core.quit()
# check if all components have finished
if not continueRoutine: # a component has requested a forced-end of Routine
break
continueRoutine = False # will revert to True if at least one component still running
for thisComponent in trialComponents:
if hasattr(thisComponent, "status") and thisComponent.status != FINISHED:
continueRoutine = True
break # at least one component has not yet finished
# refresh the screen
if continueRoutine: # don't flip if this routine is over or we'll get a blank screen
win.flip()
# -------Ending Routine "trial"-------
for thisComponent in trialComponents:
if hasattr(thisComponent, "setAutoDraw"):
thisComponent.setAutoDraw(False)
sound1.stop() # ensure sound has stopped at end of routine
trials.addData('sound1.started', sound1.tStartRefresh)
trials.addData('sound1.stopped', sound1.tStopRefresh)
trials.addData('movie1.started', movie1.tStartRefresh)
trials.addData('movie1.stopped', movie1.tStopRefresh)
# the Routine "trial" was not non-slip safe, so reset the non-slip timer
routineTimer.reset()
thisExp.nextEntry()
# completed 1 repeats of 'trials'
# Flip one final time so any remaining win.callOnFlip()
# and win.timeOnFlip() tasks get executed before quitting
win.flip()
# these shouldn't be strictly necessary (should auto-save)
thisExp.saveAsWideText(filename+'.csv')
thisExp.saveAsPickle(filename)
logging.flush()
# make sure everything is closed down
thisExp.abort() # or data files will save again on exit
win.close()
core.quit()
The problem is that using np.choice only one number is reproduced and not the entire sequence of four randomly chosen numbers without repetitions. How can I do this?
Thanks in advance

Not tested, but something like this:
FPS = 60 # Frame rate of your monitor
from random import choice
from psychopy import visual, sound
win = visual.Window()
movie = visual.MovieStim(win, 'my_file.avi')
sounds = [sound.Sound('sound1.wav'), sound.Sound('sound2.wav'), sound.Sound('sound3.wav'), sound.Sound('sound4.wav')]
frame = 1
while movie.status != visual.FINISHED:
movie.draw() # Show the next frame of the movie
if frame % FPS == 0: # If a second has passed
choice(sounds).play() # Play a random sound

ROS programming: Trying to make my turtlebot wander without running into obstacles

I'm new to ROS.
I'm trying to make my turtlebot move forward until it is within a minimal distance from an obstacle, then rotate until the path is clear, then move forward again and so on...
I wrote this code:
import rospy, sys
from stopper import Stopper
if __name__ == "__main__":
rospy.init_node("stopper_node", argv=sys.argv)
forward_speed = 0.5
angular_speed = 45
if rospy.has_param('~forward_speed'):
forward_speed = rospy.get_param('~forward_speed')
if rospy.has_param('~angular_speed'):
angular_speed = rospy.get_param('~angular_speed')
my_stopper = Stopper(forward_speed, angular_speed)
my_stopper.start_moving();
and this:
import rospy
import math
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
class Stopper(object):
def __init__(self, forward_speed, angular_speed):
self.forward_speed = forward_speed
self.angular_speed = angular_speed
self.min_scan_angle = -30 /180 * math.pi
self.max_scan_angle = 30 / 180 * math.pi
self.min_dist_from_obstacle = 1
self.keep_moving = True
self.keep_rotating = False
self.command_pub = rospy.Publisher("/cmd_vel_mux/input/teleop", Twist, queue_size=10)
self.laser_subscriber = rospy.Subscriber("scan",LaserScan, self.scan_callback, queue_size=1)
def start_moving(self):
print "started moving1"
rate = rospy.Rate(1)
rospy.loginfo("Starting to move")
while not rospy.is_shutdown() and self.keep_moving:
self.move_forward()
rate.sleep()
self.start_rotating()
def move_forward(self):
move_msg = Twist()
move_msg.linear.x = self.forward_speed
self.command_pub.publish(move_msg)
def start_rotating(self):
print "started rotating1"
rate = rospy.Rate(1)
rospy.loginfo("Starting to rotate")
while not rospy.is_shutdown() and self.keep_rotating:
self.rotate()
rate.sleep()
self.start_moving()
def rotate(self):
move_msg = Twist()
move_msg.angular.z = self.angular_speed * 2 * math.pi / 360
self.command_pub.publish(move_msg)
def scan_callback(self, scan_msg):
for dist in scan_msg.ranges:
print dist
if self.keep_moving and dist < self.min_dist_from_obstacle:
self.keep_moving = False
print "keep moving is now false"
self.keep_rotating = True
print "keep rotating is now true"
break
if self.keep_rotating and math.isnan(dist):
self.keep_rotating = False
print "keep rotating is now false"
self.keep_moving = True
print "keep moving is now true"
break
But even though I can't find any logical mistakes in it, it just does not work and occasionaly bumps into stuff. I am running it with the gazebo simulation
"turtlebot_world". I would love to get some help.
Thanks!

I have a solution with the bugs navigation algorithms, hope help you up:
with these repositories (agn_gazebo, bugs), you could bring up a Gazebo simulation with a bounded map and some obstacle and a mobile wheeled robot platform (Pioneer P3dx) which is equipped with a laser scanner (Hokuyo URG) for environmental perception.
USAGE:
After cloning from these repositories in ~/catkin_ws/src using:
git clone https://github.com/agn-7/agn_gazebo.git
git clone https://github.com/agn-7/bugs.git
Then build them with catkin_make in your catkin workspace.
After package building, you need to change the agn_gazebo/worlds/final.world file:
Open it and change all /home/agn/catkin_ws/src/... with your target path with Ctrl + F or Ctrl + H
I've opened an issue to make it as a dynamic path instead of static, but currently, I couldn't do it.
Then Bring up the simulator: roslaunch agn_gazebo gazebo.launch
Running any bugs algorithm with a position target:
rosrun bugs bug.py bug0 11 0
or
rosrun bugs bug.py bug1 11 0
or
rosrun bugs bug.py bug2 11 0

Okay, I've implemented something that should work pretty well.
I have 3 files that make the wander turtlebot work as you asked.
wander_bot.launch
Responsibility: This file is a launch file that will run the gazebo world and will store the configurable parameters.
turtlebot_node.py
Responsibility: This python file will initialize the ROS node and it will load the configurable parameters, and then it will run the code that makes the turtlebot move.
turtlebot_logic.py
Responsibility: This python file will listen to the laser, it will implement all the logic stuff to make the wander bot work.
wander_bot.launch
<launch>
<param name="/use_sim_time" value="true" />
<!-- Launch turtle bot world -->
<include file="$(find turtlebot_gazebo)/launch/turtlebot_world.launch"/>
<!-- Launch stopper node -->
<node name="wander_bot" pkg="wander_bot" type="turtlebot_node.py" output="screen">
<param name="forward_speed" type="double" value="0.2"/>
<param name="minimum_distance_from_obstacle" type="double" value="1.0"/>
<param name="rotation_speed" type="int" value="2"/>
<param name="minimum_angle" type="int" value="-30"/>
<param name="maximum_angle" type="int" value="30"/>
</node>
</launch>
turtlebot_node.py
#!/usr/bin/python
import rospy
import sys
from turtlebot_logic import RosTurtlebotLogic
def loadParams():
forwardSpeed = 0.2
rotationSpeed = 2
minDistanceFromObstacle = 1.0
minimumAngle = -30
maximumAngle = 30
if rospy.has_param('~forward_speed'):
forwardSpeed = rospy.get_param('~forward_speed')
if rospy.has_param('~rotation_speed'):
rotationSpeed = rospy.get_param('~rotation_speed')
if rospy.has_param('~minimum_distance_from_obstacle'):
minDistanceFromObstacle = rospy.get_param('~minimum_distance_from_obstacle')
if rospy.has_param('~minimum_angle'):
minimumAngle = rospy.get_param('~minimum_angle')
if rospy.has_param('~maximum_angle'):
maximumAngle = rospy.get_param('~maximum_angle')
return forwardSpeed, rotationSpeed, minDistanceFromObstacle, minimumAngle, maximumAngle
if __name__ == "__main__":
rospy.loginfo("Started program.")
rospy.init_node("stopper_node", argv=sys.argv)
forwardSpeed, rotationSpeed, minDistanceFromObstacle, minimumAngle, maximumAngle = loadParams()
rospy.loginfo("Finished import parameters.")
my_stopper = RosTurtlebotLogic(forwardSpeed, rotationSpeed, minDistanceFromObstacle, minimumAngle, maximumAngle)
my_stopper.startMoving()
turtlebot_logic.py
#!/usr/bin/python
import math
import rospy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
class RosTurtlebotLogic(object):
def __init__(self, forwardSpeed, rotationSpeed, minDistanceFromObstacle, minimumAngle, maximumAngle):
self.forwardSpeed = forwardSpeed
self.rotationSpeed = rotationSpeed
self.minDistanceFromObstacle = minDistanceFromObstacle
# Keeps the current minimum distance from obstacle from laser.
self.minimumRangeAhead = 5
# In which direction to rotate, left or right.
self.rotationDirection = 0
self.minimumIndexLaser = 360 * (90 + minimumAngle) / 90
self.maximumIndexLaser = 360 * (90 + maximumAngle) / 90 - 1
self.keepMoving = True
self.commandPub = rospy.Publisher("/cmd_vel_mux/input/teleop", Twist, queue_size=10)
rospy.Subscriber("scan", LaserScan, self.scanCallback, queue_size=10)
def startMoving(self):
rate = rospy.Rate(10)
while(not rospy.is_shutdown()):
if(self.keepMoving):
if (self.minimumRangeAhead < self.minDistanceFromObstacle):
self.keepMoving = False
else:
if(self.minimumRangeAhead > self.minDistanceFromObstacle):
self.keepMoving = True
twist = Twist()
if(self.keepMoving):
twist.linear.x = self.forwardSpeed
else:
twist.angular.z = self.rotationDirection
self.commandPub.publish(twist)
rate.sleep()
def scanCallback(self, scan_msg):
minimum = 100
index = 0
# Find the minimum distance to obstacle and we keep the minimum distance and the index.
# We need the minimum distance in order to know if we can go forward or not.
# We need the index to know in which direction to rotate.
if(not math.isnan(scan_msg.ranges[self.minimumIndexLaser])):
minimum = scan_msg.ranges[self.minimumIndexLaser]
for i in range(self.minimumIndexLaser, self.maximumIndexLaser + 1):
if(not math.isnan(scan_msg.ranges[i]) and scan_msg.ranges[i] < minimum):
minimum = scan_msg.ranges[i]
index = i
self.minimumRangeAhead = minimum
if(index <= 359):
self.rotationDirection = self.rotationSpeed
else:
self.rotationDirection = -self.rotationSpeed
The laser looks at 180 degrees, the right angle is index 0
at ranges array, forward is index 359 at ranges array, and left is index 719 at ranges array, ranges array is coming from the subscriber:
rospy.Subscriber("scan", LaserScan, self.scanCallback, queue_size=10)
The callback function is scanCallback
Well, that kind of straight forward, I don't think that I need to explain the code, it's well documented and easy to understand, for any further questions please comment here.