###### tags: `控制組`
# 1D-Sonar
writer:江承翰、徐冠東
email: neoblacktea.ee10@nycu.edu.tw、 gordondonghsu.ee10@nycu.edu.tw
## 1D-Sonar Data Sheet & Application
### Sonar Data Sheet: 可以從官網找原code與解釋等
https://bluerobotics.com/store/sensors-sonars-cameras/sonar/ping-sonar-r2-rp/
Application: 官網也有說明，此軟體可以測試sonar的值
https://docs.bluerobotics.com/ping-viewer/
### Example: 這是購買1D聲納的頁面，下面是example code(python)
```bash=
#!/usr/bin/env python
#simplePingExample.py
from brping import Ping1D
import time
import argparse
from builtins import input
##Parse Command line options
############################
parser = argparse.ArgumentParser(description="Ping python library example.")
parser.add_argument('--device', action="store", required=False, type=str, help="Ping device port. E.g: /dev/ttyUSB0")
parser.add_argument('--baudrate', action="store", type=int, default=115200, help="Ping device baudrate. E.g: 115200")
parser.add_argument('--udp', action="store", required=False, type=str, help="Ping UDP server. E.g: 192.168.2.2:9090")
args = parser.parse_args()
if args.device is None and args.udp is None:
parser.print_help()
exit(1)
# Make a new Ping
myPing = Ping1D()
if args.device is not None:
myPing.connect_serial(args.device, args.baudrate)
elif args.udp is not None:
(host, port) = args.udp.split(':')
myPing.connect_udp(host, int(port))
if myPing.initialize() is False:
print("Failed to initialize Ping!")
exit(1)
print("------------------------------------")
print("Starting Ping..")
print("Press CTRL+C to exit")
print("------------------------------------")
input("Press Enter to continue...")
# Read and print distance measurements with confidence
while True:
data = myPing.get_distance()
if data:
print("Distance: %s\tConfidence: %s%%" % (data["distance"], data["confidence"]))
else:
print("Failed to get distance data")
time.sleep(0.1)
```
以上主要拿來取得sonar量到的值-深度(data["distance"])、正確率(data["confidence"])。
## 與rpi的連結
這個聲納的傳輸方式是UART
需要去查所用的rpi型號對應的pin腳再和1D-sonar連接
distance的單位是mm
測試完功能正確就可以包成ROS package了
要注意聲納在50公分以內量測的值可能會不準確
## 使用rosserial_python傳遞數值
方法是使用rosserial_python與sonar的code，把深度(data["distance"])、正確率(data["confidence"])傳到ros上，之後就可以傳給stm32(or arduino)等控制器，進而去執行對應的動作。
ros with python: http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28python%29
以下是讓sonar publish到ros上的code的說明:
```
import rospy
from std_msgs.msg import String
from brping import Ping1D
import time
# Make a new Ping
myPing = Ping1D()
myPing.connect_serial("/dev/ttyS0", 115200) # argument 1: port (ex:/dev/ttyUSB0), argument 2: baud rate
def talker():
if myPing.initialize() is False:
rospy.loginfo("Failed to initialize Ping!")
exit(1)
rospy.loginfo("------------------------------------")
rospy.loginfo("Starting Ping..")
rospy.loginfo("Press CTRL+C to exit")
rospy.loginfo("------------------------------------")
input("Press Enter to continue...")
pub = rospy.Publisher('chatter', String, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
data = myPing.get_distance()
if not data:
rospy.loginfo("Failed to get distance data")
hello_str = f'Distance: {data["distance"]}, Confidence: {data["confidence"]}'
rospy.loginfo(hello_str)
pub.publish(hello_str)
time.sleep(0.1)
rate.sleep()
if __name__ == '__main__':
try:
talker()
except rospy.ROSInterruptException:
pass
```
經過實際下水測量，在ros顯示的值正確率高。
可參考github的說明:https://github.com/bluerobotics/ping-python
# 1.5D-Sonar(ping360)
測試ping360
```
import rospy
from std_msgs.msg import String
from brping import Ping360
import time
# Make a new Ping
myPing = Ping360()
myPing.connect_serial("/dev/ttyS0", 115200) # argument 1: port (ex:/dev/ttyUSB0), argument 2: baud rate
def talker():
if myPing.initialize() is False:
rospy.loginfo("Failed to initialize Ping!")
exit(1)
rospy.loginfo("------------------------------------")
rospy.loginfo("Starting Ping..")
rospy.loginfo("Press CTRL+C to exit")
rospy.loginfo("------------------------------------")
input("Press Enter to continue...")
pub = rospy.Publisher('chatter', String, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(10) # 10hz
myPing.set_angle(50)
while not rospy.is_shutdown():
data = myPing.get_device_data()
if not data:
rospy.loginfo("Failed to get distance data")
hello_str = f'Angle: {data["angle"]}, Data: {data["data"]}'
rospy.loginfo(hello_str)
pub.publish(hello_str)
time.sleep(0.1)
rate.sleep()
if __name__ == '__main__':
try:
talker()
except rospy.ROSInterruptException:
pass
```
ping360 function參考:https://docs.bluerobotics.com/ping-python/classbrping_1_1ping360_1_1Ping360.html
---
# 整合1D&1.5D(360):
ping360的distance取值參考: https://gist.github.com/Stormix/c4514688b69f4aad6306abe1f5c874f9
這裡用兩個1D-sonar與一個1.5D-sonar，去測量並計算yaw angle與水下x,y的座標位置。
```
#!/usr/bin/env python3.7
import rospy
from geometry_msgs.msg import Vector3
from std_msgs.msg import String
from std_msgs.msg import Float32
from brping import Ping1D
from brping import Ping360
import time
import math
def calculateRange(numberOfSamples, samplePeriod, speedOfSound, _samplePeriodTickDuration=24e-9):
# type: (float, int, float, float) -> float
"""
Calculate the range based in the duration
"""
return numberOfSamples * speedOfSound * _samplePeriodTickDuration * samplePeriod / 1
def getSonarData(sensor, angle):
"""
Transmits the sonar angle and returns the sonar intensities
Args:
sensor (Ping359): Sensor class
angle (int): Gradian Angle
Returns:
list: Intensities from -1 - 255
"""
sensor.transmitAngle(angle)
data = bytearray(getattr(sensor, '_data'))
return [k for k in data]
def calculateRange(numberOfSamples, samplePeriod, speedOfSound, _samplePeriodTickDuration=24e-9):
# type: (float, int, float, float) -> float
"""
Calculate the range based in the duration
"""
return numberOfSamples * speedOfSound * _samplePeriodTickDuration * samplePeriod / 1
def calculateSamplePeriod(distance, numberOfSamples, speedOfSound, _samplePeriodTickDuration=24e-9):
# type: (float, int, int, float) -> float
"""
Calculate the sample period based in the new range
"""
return 1 * distance / (numberOfSamples * speedOfSound * _samplePeriodTickDuration)
def adjustTransmitDuration(distance, samplePeriod, speedOfSound, _firmwareMinTransmitDuration=4):
# type: (float, float, int, int) -> float
"""
@brief Adjust the transmit duration for a specific range
Per firmware engineer:
0. Starting point is TxPulse in usec = ((one-way range in metres) * 8000) / (Velocity of sound in metres
per second)
1. Then check that TxPulse is wide enough for currently selected sample interval in usec, i.e.,
if TxPulse < (1.5 * sample interval) then TxPulse = (2.5 * sample interval)
(transmit duration is microseconds, samplePeriod() is nanoseconds)
2. Perform limit checking
Returns:
float: Transmit duration
"""
duration = 7999 * distance / speedOfSound
transmit_duration = max(
1.5 * getSamplePeriod(samplePeriod) / 1000, duration)
return max(_firmwareMinTransmitDuration, min(transmitDurationMax(samplePeriod), transmit_duration))
def transmitDurationMax(samplePeriod, _firmwareMaxTransmitDuration=499):
# type: (float, int) -> float
"""
@brief The maximum transmit duration that will be applied is limited internally by the
firmware to prevent damage to the hardware
The maximum transmit duration is equal to 63 * the sample period in microseconds
Returns:
float: The maximum transmit duration possible
"""
return min(_firmwareMaxTransmitDuration, getSamplePeriod(samplePeriod) * 63e6)
def getSamplePeriod(samplePeriod, _samplePeriodTickDuration=24e-9):
# type: (float, float) -> float
""" Sample period in ns """
return samplePeriod * _samplePeriodTickDuration
#----------------------------
# Global Variables
device = "/dev/ttyUSB0" # the serial port
baudrate = 115200
gain = 0
numberOfSamples = 200 # Number of points
transmitFrequency = 740 # Default frequency
sonarRange = 1 # in m
speedOfSound = 1500 # in m/s
# The function definitions are at the bottom
samplePeriod = calculateSamplePeriod(sonarRange, numberOfSamples, speedOfSound)
samplePeriod = int(samplePeriod)
transmitDuration = adjustTransmitDuration(sonarRange, samplePeriod, speedOfSound)
transmitDuration = int(transmitDuration)
#----------------------------
# Make a new Ping
myPing = Ping1D()
myPing1 = Ping1D()
myPing.connect_serial("/dev/ttyUSB1", 115200) # argument 1: port (ex:/dev/ttyUSB0), argument 2: baud rate
myPing1.connect_serial("/dev/ttyUSB2", 115200) # argument 1: port (ex:/dev/ttyUSB0), argument 2: baud rate
myPing360 = Ping360()
myPing360.connect_serial("/dev/ttyUSB0", 115200)
def talker():
if myPing360.initialize() and myPing.initialize() and myPing1.initialize() is False:
rospy.loginfo("Failed to initialize Ping!")
exit(1)
rospy.loginfo("------------------------------------")
rospy.loginfo("Starting Ping..")
rospy.loginfo("Press CTRL+C to exit")
rospy.loginfo("------------------------------------")
input("Press Enter to continue...")
# Set Ping360 sonar parameters
myPing360.set_gain_setting(gain)
myPing360.set_transmit_frequency(transmitFrequency)
myPing360.set_transmit_duration(transmitDuration)
myPing360.set_sample_period(samplePeriod)
myPing360.set_number_of_samples(numberOfSamples)
# Get Ping360 sonar response
angle = 0 # in Grad
pos = Vector3()
dis_x = 51500
dis_y = 24500
pos_x = 0.0
pos_y = 0.0
pos_z = 0.0
pub = rospy.Publisher('cur_pos', Vector3, queue_size=10)
rospy.init_node('sonar', anonymous=True)
rate = rospy.Rate(10) # 10hz
fix_angle = math.pi/2
plus_90 = 0
minus_90 = 0
while not rospy.is_shutdown():
data = myPing.get_distance()
data1 = myPing1.get_distance()
data360 = getSonarData(myPing360, angle)
if not data360:
rospy.loginfo("Failed to get distance data")
#lengths of triangle
threshold = 150
for detectedIntensity in data360:
if detectedIntensity >= threshold:
detectedIndex = data360.index(detectedIntensity)
break
l1 = calculateRange((1 + detectedIndex), samplePeriod, speedOfSound) #the distance of Ping360
l2 = int(data["distance"]) #the distance of Ping1D
l3 = math.sqrt( l1**2 + l2**2 - 2*l1*l2*math.cos(angle*math.pi/200.0)) #compute l3
yaw = math.acos((l2**2 + l3**2 - l1**2)/(2*l2*l3))-math.pi/2 #yaw angle, unit of radius
fix = 0
if yaw > math.pi/6:
plus_90 = plus_90 + 1
else:
plus_90 = 0
if yaw < -math.pi/6:
minus_90 = minus_90 + 1
else:
minus_90 = 0
if plus_90 > 10 and yaw < 0:
if fix == 3:
fix = 0
else:
fix = fix + 1
plus_90 = 0
if minus_90 > 10 and yaw > 0:
if fix == 0:
fix = 3
else:
fix = fix - 1
minus_90 = 0
yaw = yaw + fix*fix_angle
if fix == 0:
pos_x = (data["distance"] + 193)*abs(math.cos(yaw))
pos_y = (data1["distance"] + 262)*abs(math.cos(yaw))
elif fix == 1:
pos_x = (data1["distance"] + 262)*abs(math.cos(yaw - math.pi/2))
pos_y = dis_y - (data["distance"] + 193)*abs(math.cos(yaw - math.pi/2))
elif fix == 2:
pos_x = dis_x - (data["distance"] + 193)*abs(math.cos(yaw - math.pi))
pos_y = dis_y - (data1["distance"] + 262)*abs(math.cos(yaw - math.pi))
elif fix == 3:
pos_x = dis_x - (data1["distance"] + 262)*abs(math.cos(yaw - math.pi * 3 / 2))
pos_y = (data["distance"] + 193)*abs(math.cos(yaw - math.pi * 3 / 2))
pos.x = pos_x
pos.y = pos_y
pos.z = yaw
hello_str = f'Distance: {l2}, Confidence: {data["confidence"]}, Ping360 distance: {l1}, Position:({pos_x},{pos_y})'
rospy.loginfo(hello_str)
#pub
pub.publish(pos)
time.sleep(0.1)
rate.sleep()
if __name__ == '__main__':
try:
talker()
except rospy.ROSInterruptException:
pass
```
---
內建code(檢查初始化是否成功): /home/pi/.local/lib/python3.7/site-packages/brping/ping360.py
Google Drive
Gist
Clipboard
Sign in with Wallet
