The task I am currently working on is:
A robot that is installed with a LIDAR sensor and placed in an enclosed warehouse. The robot initially is placed at any random position in the environment. My task is to guide the robot around the warehouse using the values from the sensor, such as wall following etc.
The problem I am facing currently is that I need the robot to start at a position before it does the wall following task. This position is such that the robot first needs to get close to the nearest wall and place itself parallel to the wall, ready to begin the wall following.
I created two '.py' files, a server file and a client file.
The client file calls the server file to do the wall search task. Once the task is complete the rest of the code in the client file is to begin the wall following. I included the code with the problems in it.
msg.ranges[360] (value of front laser range) is the variable that I need help with.
# The lidar sensor has a range form 0 to 180 degress with index range 0-720. The laser at 360 is the front laser or the laser with 0 angle.
# The laser at 0 and 719 are at -90 and 90 angle.
#! /usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
from rosject.srv import findwall, findwallResponse #custom service message
def callback(msg):
r = list(msg.ranges)
minimum_value = min(r)
print(minimum_value)
index_minimum = r.index(minimum_value)
ray_angle = msg.angle_min + (index_minimum * msg.angle_increment)
#The distance and angle of an obstacle with respect to the laser scanner device must be calculated using
#The fields angle_min and angle_increment: ray_angle
print(ray_angle)
#Logic for duration of rotation
countdown_sec1 = 0
if ray_angle < 0:
twist_instance.angular.z = 0.2 #angular velocity in radians/second
else:
twist_instance.angular.z = -0.2
rotation_duration = (abs(ray_angle)/0.2) - 1
while countdown_sec1 <= rotation_duration:
pub.publish(twist_instance)
rate.sleep()
countdown_sec1 = countdown_sec1 + 1
#Once the robot faces the wall it is stopped
twist_instance.angular.z = 0
twist_instance.linear.x = 0
pub.publish(twist_instance)
rate.sleep()
print('front distance',msg.ranges[360]) #once stopped, I print the range of the front laser facing the wall
#This is where I faced the problem. the value that prints for the front laser is incorrect. I am unable to understand why.
#The following code is just to move the robot forward until it's within a certain distance from the wall.
difference = 0.3
forward_duration = round(difference/0.1)
countdown_sec2 = 0
twist_instance.linear.x = 0.1
twist_instance.angular.z = 0
while countdown_sec2 <= forward_duration:
pub.publish(twist_instance)
rate.sleep()
countdown_sec2 = countdown_sec2 + 1
twist_instance.linear.x = 0
pub.publish(twist_instance)
print('final distance', msg.ranges[360]) #Even here once the task is complete the the final value remains the same and incorrect.
# I let the callback run again and even then the value for the front laser does not change
def my_callback(request):
print('i am working boy')
my_response = findwallResponse()
my_response.wallfound = True
return my_response
if __name__ == '__main__':
rospy.init_node('findwall_node')
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
sub = rospy.Subscriber('/scan', LaserScan, callback)
twist_instance = Twist()
rate = rospy.Rate(1)
my_service = rospy.Service('/find_wall', findwall, my_callback)
rospy.spin()
Where am I going wrong, why wont the value for msg.ranges[360] not change when the loop is run again by rospy.spin()?
msg won't change as you're still inside callback(msg). In your code, callback(msg) will be called everytime sub will receive a LiDAR scan message.
I suggest that you separate the code that receives the message from the code that waits and publishes, as you will lose laser scan messages if your node is still processing a msg.
Here is a way to do it in ROS1:
#!/usr/bin/env python
import rospy
from std_msgs.msg import Int64, String
class MyNode:
# CLASS_VARIABLES
def __init__(self, start_value=0):
# INSTANCE_VARIABLES
self.counter = start_value
pass
def counter_callback(self, msg):
counter_data = msg.data
self.counter += counter_data
if __name__ == "__main__":
rospy.init_node("my_node")
rate = rospy.Rate(2)
node = MyNode()
sub = rospy.Subscriber("mynode_pub_topic", Int64, node.counter_callback)
string_pub = rospy.Publisher("mynode_pub_topic", String, queue_size=10)
string_instance = String()
while not rospy.is_shutdown():
string_instance.data = str(node.counter)
string_pub.publish(string_instance)
rate.sleep()
Other improvements could be to use publisher and subscribers as class attributes, as it is now done in ROS2.
Related
I have a program that is being executed on reboot via crontab. The script and programs work as expected if I run the script from terminal manually myself, but on running the script on boot its as if my RoverDriver.py program is being killed for no reason with no errors.
#reboot sleep 15 && sh /home/pi/Desktop/Rover-Driver/launcher.sh >/home/pi/Desktop/Rover-Driver/logs/cronlog 2>&1
The shell script is:
cd /
cd /home/pi/Desktop/Rover-Driver
sudo python3 bluetooth.py
cd /
The bluetooth.py program checks to see if our bluetooth wireless controller is connected to the pi, if so start the driver program
import pexpect
import time
attempts = 0
while attempts < 30:
child = pexpect.spawn('bluetoothctl')
child.sendline('paired-devices')
output = child.read_nonblocking(size = 200, timeout = -1).decode('utf-8')
target = 'Xbox Wireless Controller'
if output.find(target > -1:
print("Target Found")
print(pexpect.run('python RoverDriver.py'))
break
else:
print("No target found")
attempts = attempts + 1
time.sleep(1)
The RoverDiver.py is:
import pyfirmata
import pygame
import time
from pygame.locals import *
pygame.init()
# controller setup
joysticks = []
for i in range(pygame.joystick.get_count()):
joysticks.append(pygame.joystick.Joystick(i))
joysticks[-1].init()
controller = joysticks[0]
board = pyfirmata.ArduinoMega('/dev/ttyACM0')
# setting pins
onboard_led = board.get_pin('d:13:o')
l_motor_pin = board.get_pin('d:11:p')
r_motor_pin = board.get_pin('d:12:p')
motor_toggle_pin = board.get_pin('d:24:o')
# constantly updates statuses for example if reading analog input from potentiometer
it = pyfirmata.util.Iterator(board)
it.start()
motor_toggle_pin.write(0)
l_motor_pin.write(.49804)
r_motor_pin.write(.49804)
print("Have not entered while loop yet")
while(1):
pygame.event.pump()
# puts rover in "stop" state if no input, avoids rover from "running away"
motor_toggle_pin.write(0)
l_motor_pin.write(.49804)
r_motor_pin.write(.49804)
if(controller.get_button(11)):
print("Exiting...")
exit()
# divider ensure only half power given unless "A" button pressed down
divider = 0.125
reverse = False
right_power = controller.get_axis(4)
left_power = controller.get_axis(5)
# "A" button pressed, give full power
if(controller.get_button(0)):
divider = 0.25
right_power = divider * (right_power + 1)
left_power = divider * (left_power + 1)
# Bumpers are pressed, go reverse. Must be doing both to avoid breaking bot
if(controller.get_button(6) and controller.get_button(7)):
left_power = 0.5 - left_power
right_power = 0.5 - right_power
reverse = True
else:
left_power = 0.5 + left_power
right_power = 0.5 + right_power
# send motors their values
motor_toggle_pin.write(1)
l_motor_pin.write(left_power)
r_motor_pin.write(right_power)
print(f"L Power:{left_power} |R Power:{right_power}")
# avoid cpu overloading
time.sleep(.05)
# after exiting while loop, "turn off" motors
motor_toggle_pin.write(0)
l_motor_pin.write(.49804)
r_motor_pin.write(.49804)
print("End.")
I am expecting a rather large blob of text in the logs (because of the print statement of power levels), or at least the before/after print statements of the while loop however all i get is this:
Target Found.
b'pygame 1.9.4post1\r\nHello from the pygame community. https://www.pygame.org/contribute.html\r\n'
I know that its formatted that way because I am not decoding it. I am hypothesizing it has something to do with pygame but not sure what?
EDIT: To anyone who comes across this post. I switched my controls completely and no longer even rely on pygame. I switched to connecting to the pi via sockets and sending command strings over. This still needed to be setup on boot so I tried using crontab (unsuccessfully again) and instead found out about systemd which works beautifully.
I am designing a new time/score keeper for an air hockey table using a PyBoard as a base. My plan is to use a TM1627 (4x7seg) for time display, rotary encoder w/ button to set the time, IR and a couple 7segs for scoring, IR reflector sensors for goallines, and a relay to control the fan.
I'm getting hung up trying to separate the clock into its own thread while focusing on reading the sensors. Figured I could use uasyncio to split everything up nicely, but I can't figure out where to put the directives to spin off a thread for the clock and eventually the sensors.
On execution right now, it appears the rotary encoder is assigned the default value, no timer is started, the encoder doesn't set the time, and the program returns control to REPL rather quickly.
Prior to trying to async everything, I had the rotary encoder and timer working well. Now, not so much.
from rotary_irq_pyb import RotaryIRQ
from machine import Pin
import tm1637
import utime
import uasyncio
async def countdown(cntr):
# just init min/sec to any int > 0
min = sec = 99
enableColon = True
while True:
# update the 4x7seg with the time remaining
min = abs(int((cntr - utime.time()) / 60))
sec = (cntr - utime.time()) % 60
#print(str(), str(sec), sep=':' )
enableColon = not enableColon # alternately blink the colon
tm.numbers(min, sec, colon = enableColon)
if(min + sec == 0): # once both reach zero, break
break
await uasyncio.sleep(500)
X1 = pyb.Pin.board.X1
X2 = pyb.Pin.board.X2
Y1 = pyb.Pin.board.Y1
Y2 = pyb.Pin.board.Y2
button = pyb.Pin(pyb.Pin.board.X3, pyb.Pin.IN)
r = RotaryIRQ(pin_num_clk=X1,
pin_num_dt=X2,
min_val=3,
max_val=10,
reverse=False,
range_mode=RotaryIRQ.RANGE_BOUNDED)
tm = tm1637.TM1637(clk = Y1, dio = Y2)
val_old = val_new = 0
while True:
val_new = r.value()
if(val_old != val_new):
val_old = val_new
print(str(val_new))
if(button.value()): # save value as minutes
loop = uasyncio.get_event_loop()
endTime = utime.time() + (60 * val_new)
loop.create_task(countdown(endTime))
r.close() # Turn off Rotary Encoder
break
#loop = uasyncio.get_event_loop()
#loop.create_task(countdown(et))
#loop.run_until_complete(countdown(et))
I'm sure it's something simple, but this is the first non-CLI python script I've done, so I'm sure there are a bunch of silly mistakes. Any assistance would be appreciated.
I'm using python 3.6 with Jupyter and Anaconda.
The script goes move by move in a chess game from a PGN file, and it returns a score. That works great.
The problem comes when the score returned is "-M8", which means black can give checkmate in 8 moves.
But the following line:
(handler.info["score"][1].cp)
returns "none" instead of "-M8" as expected.
Script:
import chess
import chess.uci
import chess.pgn
import sys
with open('E:\PGN Files\Fischer.pgn') as f:
handler = chess.uci.InfoHandler()
engine = chess.uci.popen_engine('C:\Program Files (x86)\Arena\Engines\stockfish_9_x64.exe') #give correct address of your engine here
engine.info_handlers.append(handler)
for number in range(10):
game = chess.pgn.read_game(f)
m=0
while not game.is_end():
m=m+1
node = game.variations[0]
board = game.board()
game = node
engine.position(board)
#Set your evaluation time, in ms:
evaltime = 5000 #so 5 seconds
evaluation = engine.go(movetime=evaltime)
score =(handler.info["score"][1].cp)
print(str(score))
I have raspberry pi 3 and trying to create smart green house model. This model should open window if temperature is too hight.
I am new writing codes in Python, found several examples: 1. for temperature sensor and 2. for servo motor to rotating.
Could anyone help me with servo motor? I would like to move servo for example to 30° if temperature is 20°C, if it is 21°C move 40° servo and so on.
I have Python code:
import sys
import Adafruit_DHT
import time
import wiringpi
sensor_args = { '11': Adafruit_DHT.DHT11,
'22': Adafruit_DHT.DHT22,
'2302': Adafruit_DHT.AM2302 }
if len(sys.argv) == 3 and sys.argv[1] in sensor_args:
sensor = sensor_args[sys.argv[1]]
pin = sys.argv[2]
else:
print('usage: sudo ./Adafruit_DHT.py [11|22|2302] GPIOpin#')
print('example: sudo ./Adafruit_DHT.py 2302 4 - Read from an AM2302
connected to GPIO #4')
sys.exit(1)
humidity, temperature = Adafruit_DHT.read_retry(sensor, pin)
if humidity is not None and temperature is not None:
print('Temp={0:0.1f}* Humidity={1:0.1f}%'.format(temperature, humidity))
else:
print('Failed to get reading. Try again!')
sys.exit(1)
temp=temperature
text_file = open("output.txt", "w")
text_file.write("%s" %(temp))
text_file.close()
wiringpi.wiringPiSetupGpio()
wiringpi.pinMode(18,wiringpi.GPIO.PWM_OUTPUT)
wiringpi.pwmSetMode(wiringpi.GPIO.PWM_MODE_MS)
wiringpi.pwmSetClock(192)
wiringpi.pwmSetRange(2000)
delay_period = 0.01
while temp==20.0:
for pulse in range(50,250,1):
wiringpi.pwmWrite(18,50)
time.sleep(delay_period)
for pulse in range(250,50,-1):
wiringpi.pwmWrite(18,pulse)
time.sleep(delay_period)
Part about servo motor ir example I found on the internet. I need to replace "while" to "if". I tried by myself, but rotor all the time spin to the same angle.
Can anyone help me with this little part of code?
Second question, how can I run this command in terminal "sudo python servo.py 11 17" on raspberry pi automatically in every 10 mminutes and when raspberry pi is turned on?
Thanks for your help!
The pulse is how you are controlling the position of the servo in your code:
wiringpi.pwmWrite(18,pulse)
See this example:
https://learn.adafruit.com/adafruits-raspberry-pi-lesson-8-using-a-servo-motor?view=all
Depending on your servo, a pulse value of 100 move the servo all the way left (closed in this example), and 200 all the way to the right (open in this example). You need to find these values by reading the datasheet or by experimentation. Once you have these, here is how you would set the position of the servo:
min_servo_val = 100
max_servo_val = 200
wiringpi.pwmWrite(18, min_servo_val) # Move all the way left
time.sleep(1) # Wait a second
wiringpi.pwmWrite(18, max_servo_val) # Move all the way right
Now you can either write a function that translates temperature to a servo position between min_servo_val and max_servo_val, or use a simple if statement. Here is an example of a function to translate temp into a pulse (servo position):
def get_servo_position(temp):
min_servo_val = 100 # Pulse value at which window is all the way closed closed
max_servo_val = 200 # Pulse value at which window is all the way open
full_closed_temp = 20.0 # Temperature at which window is completely closed
full_open_temp = 26.0 # Temperature at which window is completely open
if temp <= full_closed_temp:
return min_servo_val
elif temp >= full_open_temp:
return max_servo_val
else:
return ((temp - full_closed_temp) / (full_open_temp - full_closed_temp)) * (max_servo_val - min_servo_val) + min_servo_val
Now you can do:
print get_servo_position(19) # 100 all the way closed
print get_servo_position(22) # 133.3, or 33% of the way between min_servo_val and max_servo_val
print get_servo_position(25) # 183.3, or 83% of the way between min_servo_val and max_servo_val
print get_servo_position(27) # 200 all the way open
Now you need a loop that checks the temperature every ten minutes and adjusts the servo position. Something like this:
while True:
humidity, temp = Adafruit_DHT.read_retry(sensor, pin) # Get temperature.
position = get_servo_position(temp) # Get the servo position
wiringpi.pwmWrite(18,position) # Move to the position
time.sleep(10*60) # Wait 10 minutes
Putting it all together, your script should look something like this:
import sys
import Adafruit_DHT
import time
import wiringpi
dht_pin = 17 # GPIO conencted to DHT
servo_pin = 18 # GPIO connected to servo
dht_sensor = Adafruit_DHT.DHT11 # Put your sensor here, or set it from command line args
min_servo_val = 100 # Pulse value at which window is all the way closed closed
max_servo_val = 200 # Pulse value at which window is all the way open
full_closed_temp = 20.0 # Temperature at which window is completely closed
full_open_temp = 26.0 # Temperature at which window is completely open
def get_servo_position(temp):
if temp <= full_closed_temp:
return min_servo_val
elif temp >= full_open_temp:
return max_servo_val
else:
return ((temp - full_closed_temp) / (full_open_temp - full_closed_temp)) * (max_servo_val - min_servo_val) + min_servo_val
def main_loop():
while True:
humidity, temp = Adafruit_DHT.read_retry(dht_sensor, dht_pin) # Get temperature.
position = get_servo_position(temp) # Get the servo position
wiringpi.pwmWrite(18,position) # Move to the position
time.sleep(10*60) # Wait 10 minutes
if __name__ == '__main__':
# If you need to get command line arguments, do it below, otherwise just set the pins and other settings at the top of this script.
# For example...
# dht_pin = sys.argv[1]
# servo_pin = sys.argv[2]
# Set up servo
wiringpi.wiringPiSetupGpio()
wiringpi.pinMode(18,wiringpi.GPIO.PWM_OUTPUT)
wiringpi.pwmSetMode(wiringpi.GPIO.PWM_MODE_MS)
wiringpi.pwmSetClock(192)
wiringpi.pwmSetRange(2000)
# Enter main loop
main_loop()
Note that I left out command line arg parsing. If you need those you can add them right after if __name__ == '__main__':
Finally, making a script run on startup is a well covered topic:
Start shell script on Raspberry Pi startup
apologies if this isn't the right place to ask, but I did some searching and couldn't find much to point me in the right direction. I wasn't quite sure what to search for. I am a novice with python and programming in general, but usually can do enough googling and stealing other code snippets to get my projects running. However I'm at a bit of a roadblock here.
I need to control an Adafruit DotStar lightstrip with a flask web browser app. I've been able to get the flask app working, I've done a simple proof of concept with turning an LED on and off etc., and I can start my lightstrip script but the code I'm trying to run for the lightstrip needs to loop continuously and still be able to change "modes". I have several different images that display on the light strip and I would like to be able to select which one(s) is/are playing, but for now mainly I would just like to be able to start and stop a "shuffle all" mode. If I run the module in a while loop it just loops forever and I can't change the argument to a different "mode". I built a simple script based on Adafruit's DotStar library (specifically the image persistence of vision script, I'm just using PNG images as the map for the different lightstrip "shows").
It all currently works except it only runs each mode once obviously. I had it all in a while loop and it just looped the first selected mode forever and I was unable to turn it off or switch modes. I also thought maybe I should use multiprocessing, and I looked into getting that working, but I couldn't figure out how to stop a process once it started.
Here is the light strip script:
(the 'off' mode is just a black image. I'm sure theres a cleaner way to do this but I'm not sure on how to do that either)
import Image
from dotstar import Adafruit_DotStar
import random
def lightstrip(mode):
loopLength = 120 #loop length in pixels
fade = "/home/pi/lightshow/images/fade.png"
sparkle = "/home/pi/lightshow/images/sparkle.png"
steeplechase = "/home/pi/lightshow/images/steeplechase.png"
bump = "/home/pi/lightshow/images/bump.png"
spaz = "/home/pi/lightshow/images/spaz.png"
sine = "/home/pi/lightshow/images/sine.png"
bounce = "/home/pi/lightshow/images/bounce.png"
off = "/home/pi/lightshow/images/null.png"
numpixels = 30
datapin = 23
clockpin = 24
strip = Adafruit_DotStar(numpixels, 100000)
rOffset = 3
gOffset = 2
bOffset = 1
strip.begin()
if mode == 1:
options = [fade, sparkle, steeplechase, bump, spaz, sine, bounce]
print "Shuffling All..."
if mode == 2:
options = [bump, spaz, sine, bounce]
print "Shuffling Dance..."
if mode == 3:
options = [fade, sparkle, steeplechase]
print "Shuffling Chill..."
if mode == 0:
choice = off
print "Lightstrip off..."
if mode != 0:
choice = random.choice(options)
print "Loading..."
img = Image.open(choice).convert("RGB")
pixels = img.load()
width = img.size[0]
height = img.size[1]
print "%dx%d pixels" % img.size
# Calculate gamma correction table, makes mid-range colors look 'right':
gamma = bytearray(256)
for i in range(256):
gamma[i] = int(pow(float(i) / 255.0, 2.7) * 255.0 + 0.5)
# Allocate list of bytearrays, one for each column of image.
# Each pixel REQUIRES 4 bytes (0xFF, B, G, R).
print "Allocating..."
column = [0 for x in range(width)]
for x in range(width):
column[x] = bytearray(height * 4)
# Convert entire RGB image into column-wise BGR bytearray list.
# The image-paint.py example proceeds in R/G/B order because it's counting
# on the library to do any necessary conversion. Because we're preparing
# data directly for the strip, it's necessary to work in its native order.
print "Converting..."
for x in range(width): # For each column of image...
for y in range(height): # For each pixel in column...
value = pixels[x, y] # Read pixel in image
y4 = y * 4 # Position in raw buffer
column[x][y4] = 0xFF # Pixel start marker
column[x][y4 + rOffset] = gamma[value[0]] # Gamma-corrected R
column[x][y4 + gOffset] = gamma[value[1]] # Gamma-corrected G
column[x][y4 + bOffset] = gamma[value[2]] # Gamma-corrected B
print "Displaying..."
count = loopLength
while (count > 0):
for x in range(width): # For each column of image...
strip.show(column[x]) # Write raw data to strip
count = count - 1
And the main.py script for running the web app:
from flask import *
from lightshow import *
from multiprocessing import Process
import RPi.GPIO as GPIO
import Image
from dotstar import Adafruit_DotStar
import random
import time
app = Flask(__name__)
#app.route("/")
def hello():
return render_template('index.html')
#app.route("/lightstrip/1", methods=['POST'])
def shuffleall():
lightstrip(1)
return ('', 204)
#app.route("/lightstrip/2", methods=['POST'])
def shuffledance():
lightstrip(2)
return ('', 204)
#app.route("/lightstrip/3", methods=['POST'])
def shufflechill():
lightstrip(3)
return ('', 204)
#app.route("/lightstrip/0", methods=['POST'])
def off():
lightstrip(0)
return ('', 204)
if __name__ == "__main__":
app.run(host='0.0.0.0', debug=True)
Again I'm at a bit of a loss here, this may be simple fix or I may be approaching it totally wrong but any and all help would be appreciated. I am a complete beginner to approaching a problem like this. Thank you
Here's an example showing how to start and stop processes using multiprocessing and psutil. In this example the task_runner kills any running processes before starting a new one.
from flask import Flask
import multiprocessing
import psutil
app = Flask(__name__)
def blink(var):
while True:
# do stuff
print(var)
def task_runner(var):
processes = psutil.Process().children()
for p in processes:
p.kill()
process = multiprocessing.Process(target=blink, args=(var,))
process.start()
#app.route("/red")
def red():
task_runner('red')
return 'red started'
#app.route("/blue")
def blue():
task_runner('blue')
return 'blue started'
if __name__ == "__main__":
app.run()
For your question, the task_runner would look something like:
def task_runner(mode):
processes = psutil.Process().children()
for p in processes:
p.kill()
process = multiprocessing.Process(target=lightstrip, args=(mode,))
process.start()