---
# System prepended metadata

title: 1D-Sonar & 1.5D-Sonar
tags: [控制組]

---

###### 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的座標位置。
![](https://hackmd.io/_uploads/rJl4TkmTn.png)



```
#!/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