def __init__(self, device_port, baudrate=115200, confidence_threshold=90):
"""
:param device_port: The serial port that the Ping echosonar is connected to.
:param baudrate: The baudrate of the serial port that we are communicating over. Default is set to 115200.
:param confidence_threshold: The desired confidence threshold, i.e. the minimum confidence necessary
for us to use the given value by the Ping echosonar. Default is set to 90%.
"""
# If given confidence threshold is too high, default to 100.
if confidence_threshold > 100:
confidence_threshold = 100
# If given confidence threshold is too low, default to 0.
if confidence_threshold < 0:
confidence_threshold = 0
self.confidence_threshold = confidence_threshold
self.ping = Ping1D(device_port, baudrate)
# Raise an exception if the Ping cannot initialize.
if self.ping.initialize() is False:
raise Exception(
"Ping device failed to initialize. Verify device port is accurate\
and baudrate is a valid value.")
def __init__(self, device):
self.ping = Ping1D()
if device is not None:
self.ping.connect_serial(device, 115200)
if self.ping.initialize() is False:
rospy.logerror("Ping Sonar failed to initialize")
exit(1)
data = self.ping.get_general_info()
rospy.loginfo("Ping1D Info")
rospy.loginfo(
"- Firmware version: %s.%s" %
(data["firmware_version_major"], data["firmware_version_minor"]))
rospy.loginfo("- Ping Interval: %s ms" % data["ping_interval"])
rospy.loginfo("- Gain: %s - Mode: %s" %
(data["gain_setting"], data["mode_auto"]))
self.ping_rate = 1000 / int(data["ping_interval"])
self.history_pub = rospy.Publisher('history', Image, queue_size=3)
self.profile_pub = rospy.Publisher('profile',
UInt8MultiArray,
queue_size=3)
self.distance_pub = rospy.Publisher('distance', Range, queue_size=3)
self.history = Sonar.MAX_HISTORY * [Sonar.MAX_RESOLUTION * [0.0]]
self.bridge = CvBridge()
pass
def setup_ping(self):
# Create Ping1D object
self.ping = Ping1D("/dev/ttyUSB0", 115200)
# Initialize
if self.ping.initialize() is False:
print("Failed to initialize Ping!")
exit(1)
# Speed of sound (1500m/s)
if self.ping.set_speed_of_sound(1500000) is False:
print("Failed to set speed of sound")
exit(1)
# Auto-mode
if self.ping.set_mode_auto(1) is False:
print("Failed to set speed of auto mode")
exit(1)
# Interval (10Hz)
if self.ping.set_ping_interval(10) is False:
print("Failed to set ping interval")
exit(1)
# Enable
if self.ping.set_ping_enable(1) is False:
print("Failed to enable Ping1D")
exit(1)
def __init__(self, USB_device_adress=USB.SONAR):
self.logger = DEFLOG.DISTANCE_LOCAL_LOG
if DEFLOG.DISTANCE_LOCAL_LOG:
self.logger = Logger(filename='front_distance',
directory=DEFLOG.LOG_DIRECTORY)
self.sensor = Ping1D(USB_device_adress, 115200)
self.log("connected to the device")
self.client = Client(DISTANCE_DRIVER_PORT)
self.log("connected to the server")
def sonar_publisher():
#parser = argparse.ArgumentParser(description="Ping python library example.")
#parser.add_argument('--device', action="store", required=True, type=str, help="Ping device port.")
#parser.add_argument('--baudrate', action="store", type=int, default=115200, help="Ping device baudrate.")
#args = parser.parse_args()
#Make a new Ping
#myPing = Ping1D(args.device, args.baudrate)
myPing = Ping1D("/dev/ttyUSB0", 115200)
if myPing.initialize() is False:
print("Failed to initialize Ping!")
exit(1)
#input("Press Enter to continue...")
pub_dis = rospy.Publisher('sonar/distance', Int64, queue_size=10)
pub_conf = rospy.Publisher('sonar/confidence', Int64, queue_size=10)
pub_trans_dura = rospy.Publisher('sonar/transmit_duration',
Int64,
queue_size=10)
pub_ping_num = rospy.Publisher('sonar/ping_number', Int64, queue_size=10)
pub_scan_start = rospy.Publisher('sonar/scan_start', Int64, queue_size=10)
pub_scan_length = rospy.Publisher('sonar/scan_length',
Int64,
queue_size=10)
# pub_gain_setting = rospy.Publisher('sonar/gain_setting', Int64, queue_size=10)
pub_speed_of_sound = rospy.Publisher('sonar/speed_of_sound',
Int64,
queue_size=10)
pub_voltage = rospy.Publisher('sonar/voltage', Int64, queue_size=10)
pub_ping_interval = rospy.Publisher('sonar/ping_interval',
Int64,
queue_size=10)
rospy.init_node('sonar_publisher', anonymous=True)
r = rospy.Rate(60)
while not rospy.is_shutdown():
data = myPing.get_distance()
data2 = myPing.get_speed_of_sound()
data3 = myPing.get_general_info()
if data:
print("Distance: %s\tConfidence: %s%%" %
(data["distance"], data["confidence"]))
#time.sleep(0.1)
pub_dis.publish(data["distance"])
pub_conf.publish(data["confidence"])
pub_ping_num.publish(data["ping_number"])
pub_scan_start.publish(data["scan_start"])
pub_scan_length.publish(data["scan_length"])
#pub_gain_setting.publish(data["gain_setting"])
if data2:
pub_speed_of_sound.publish(data2["speed_of_sound"])
r.sleep()
if data3:
pub_voltage.publish(data3["voltage_5"])
pub_voltage.publish(data3["ping_interval"])
def publishData():
# Instance of the message so its values can be continually changed then published
ping_msg = pingMessage()
# Makes references to these two variables in this function point towards the global variables
global cachedFakeDistance
global cachedFakeConfidence
# Sets up a server that's basically publishing fake data just incase we need it
if shouldEmulateData:
master, slave = pty.openpty()
s_name = os.ttyname(slave)
ser = Serial(s_name, 2400, timeout=1)
publisherThread = threading.Thread(target=publishFakeData, args=[ser])
publisherThread.start()
# Sets up another thread that's always listening to it just incase we need the values
listenerThread = threading.Thread(target=readFakeData, args=[master])
listenerThread.start()
distanceData = dict()
while not rospy.is_shutdown():
# If we're reading from the ping, get data from the actual library function
if shouldEmulateData:
distanceData['distance'] = cachedFakeDistance / 1000
distanceData['confidence'] = cachedFakeConfidence
# Otherwise, set up distanceData's values as the last cached fake values from the stream
else:
distanceData = Ping1D.get_distance_simple(myPing)
# Basic error checking
if distanceData is not None:
# Raw Data is in millimeters, data published to our topic should be in meters
ping_msg.distance = distanceData['distance']
ping_msg.confidence = distanceData['confidence']
pub.publish(ping_msg)
# Otherwise, distance data somehow screwed up this iteration around
else:
print(
"get_distance_simple() returned an invalid value. Skipping iteration."
)
rospy.sleep(.1)
def __init__(self):
try:
print("ACLib constructor")
self._pinger1 = Ping1D('/dev/ttyUSB0', 115200)
if self._pinger1.initialize() is False:
print("Failed to initialize Ping 1!")
exit(1)
time.sleep(1)
self._pinger2 = Ping1D('/dev/ttyUSB1', 115200)
#if self._pinger2.initialize() is False:
# print("Failed to initialize Ping 2!")
# exit(1)
#time.sleep(1)
self._pinger3 = Ping1D('/dev/ttyUSB2', 115200)
if self._pinger3.initialize() is False:
print("Failed to initialize Ping 3!")
exit(1)
print("ACLib constructor done")
except Exception as e:
print(e)
def __init__(self):
device = rospy.get_param("~device", "/dev/ttyAMA0")
baudrate = rospy.get_param("~baudrate", 115200)
sample_rate = rospy.get_param("~sample_rate", 5.0)
speed_of_sound = rospy.get_param(
"~speed_of_sound", 1500.0
) # 1500m/s is the speed of sound in salt water, 1435 for fresh, 343 for air
self.auto_mode = rospy.get_param("~auto_mode", True)
self.range_min = rospy.get_param("~range_min", 0.1)
self.range_max = rospy.get_param("~range_max", 30.0)
self.range_pub = rospy.Publisher('ping', Range, queue_size=1)
self.ping_pub = rospy.Publisher('altimeter', PingRange, queue_size=1)
self.out_of_range_pub = rospy.Publisher('ping_out_of_range',
Bool,
queue_size=1)
self.field_of_view = 30 * np.pi / 180.0
# Old initialisation...
self.pinger = Ping1D(device, baudrate)
# New initialisation
#self.pinger = Ping1D()
#self.pinger.connect_serial(device, baudrate)
if self.pinger.initialize() is False:
rospy.logerr("Failed to initialize Ping")
exit(1)
self.pinger.set_speed_of_sound(int(speed_of_sound))
if not self.auto_mode:
self.pinger.set_mode_auto(0)
scan_length = (self.range_max - self.range_min) * 1000.0
self.pinger.set_range(self.range_min * 1000., scan_length)
rospy.loginfo(
"Setting scan range to [%f, %f] (m)" % self.range_min,
self.range_max)
self.count = 0
self.set_range_service = rospy.Service('set_pinger_range',
floatpi.srv.SetPingerRange,
self.setRangeCallback)
self.set_mode_service = rospy.Service('set_pinger_mode',
std_srvs.srv.SetBool,
self.setModeCallback)
print("Started services")
rospy.Timer(rospy.Duration(1 / sample_rate), self.timerCallback)
def __init__(self):
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing " % (self.node_name))
self.p30Front = Ping1D()
self.p30Left = Ping1D()
self.p30Right = Ping1D()
self.p30Down = Ping1D()
self.check_port()
self.sonar_front = SonarData()
self.sonar_left = SonarData()
self.sonar_right = SonarData()
self.sonar_down = SonarData()
self.pub_sonar = rospy.Publisher("sonar", SonarDataList, queue_size=1)
if (self.p30Front.initialize() and self.p30Left and self.p30Right
and self.p30Down) is False:
rospy.loginfo("Failed open the P30 sensors")
exit(1)
rospy.Timer(rospy.Duration(0.1), self.event_cb)
# Todo - Make sure this doesn't cause errors due to me running off of a custom dictionary here instead of using a variable instantiated directly from the config file
return currentCfg
# Setting up the node and the publisher for the Ping's data with ROS
rospy.init_node('ping_viewer')
pub = rospy.Publisher('/ping/raw', pingMessage, queue_size=10)
# Ping is connected and readable, so we start the dynamic reconfig server
srv = Server(PingDriverConfig, reconfigure_cb)
# Initializing Ping and error-checking
myPing = None # Delcared here to retain global scope
if not shouldEmulateData:
myPing = Ping1D(port, 115200)
if not myPing.initialize():
rospy.logwarn(
"Failed to initialize Ping! This probably means that it couldn't find the correct serial port or something similar. Fatal error."
)
def outputStartupPingValues():
# ROS Initialization Debug Information
rospy.loginfo("Device ID: " + str(myPing.get_device_id()['device_id']))
rospy.loginfo("Major Firmware Version: " +
str(myPing.get_general_info()['firmware_version_major']))
rospy.loginfo("Minor Firmware Version: " +
str(myPing.get_general_info()['firmware_version_minor']))
rospy.loginfo("Device Supply Voltage (in mV): " +
parser = argparse.ArgumentParser(description="Ping python library example.")
parser.add_argument('--device',
action="store",
required=True,
type=str,
help="Ping device port.")
parser.add_argument('--baudrate',
action="store",
type=int,
default=115200,
help="Ping device baudrate.")
args = parser.parse_args()
#Make a new Ping
myPing = Ping1D(args.device, args.baudrate)
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:
from brping import Ping1D
import time
ping = Ping1D("/dev/ttyUSB1", 115200)
ping.initialize()
ping.set_speed_of_sound(1450000)
plik = open("daneSonar", "w")
try:
while True:
dane = ping.get_distance()
if dane:
plik.write(str(dane) + '\n')
print(str(dane) + '\n')
else:
plik.write("Blad\n")
time.sleep(0.035)
finally:
plik.close()
worksheet.write(1, 4, 'Sumbu Z')
worksheet.write(1, 5, 'Confidence')
# Start from the first cell. Rows and columns are zero indexed.
row = 2
col = 2
if __name__ == '__main__':
rospy.init_node('dataexcel', anonymous=True)
rospy.Subscriber("/mavros/global_position/global", NavSatFix, callback)
rate = rospy.Rate(2)
pZ = rospy.Publisher("/makarax/PosZ", String, queue_size=10)
PosisiX = None
PosisiY = None
PosisiZ = None
myPing = Ping1D("/dev/ttyACM1", 115200)
data = myPing.get_distance()
while not rospy.is_shutdown():
data = myPing.get_distance()
worksheet.write(row, col, PosisiX)
worksheet.write(row, col + 1, PosisiY)
if data:
worksheet.write(row, col + 2, data["distance"])
worksheet.write(row, col + 3, data["confidence"])
rospy.loginfo(data["distance"])
rospy.loginfo(data["confidence"])
pZ.publish(str(data["distance"]))
row += 1
rate.sleep()
workbook.close()
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("------------------------------------")
import tsys01
from time import sleep
import ms5837
import pygame
from brping import Ping1D
import serial
global ser
sensor1 = tsys01.TSYS01()
sensor2 = ms5837.MS5837_02BA(0)
sensor3 = Ping1D("/dev/ttyUSB0", 115200)
sensor2.setFluidDensity(ms5837.DENSITY_FRESHWATER)
pygame.joystick.quit()
pygame.joystick.init()
pygame.init()
if pygame.joystick.get_count() == 0:
print("reconnect joystick")
pygame.joystick.quit()
pygame.joystick.init()
def joystickInit():
js_1 = pygame.joystick.Joystick(0)
js_1_name = js_1.get_name()
print(js_1_name)
print("I got the joystick WOOOW")
return js_1
ser = serial.Serial("/dev/ttyACM0", 9600)
def main():
"""Main function
Enumerate new ping devices
"""
output = ""
# Remove any previously created links
try:
output = subprocess.check_output(["rm", "-rf", "/dev/serial/ping"])
except subprocess.CalledProcessError as exception:
print(exception)
# Look for connected serial devices
try:
output = subprocess.check_output("ls /dev/serial/by-id", shell=True)
except subprocess.CalledProcessError as exception:
print(exception)
exit(1)
pings_found = 0
# Look at each serial device, probe for ping
for dev in output.split('\n'):
if not dev:
continue
byiddev = "/dev/serial/by-id/" + dev
print("Looking for Ping at %s" % byiddev)
new_ping = Ping1D(byiddev)
if not new_ping.initialize():
continue
try:
device_id = new_ping.get_device_id()
firmware_version = new_ping.get_firmware_version()
if device_id and firmware_version:
# ex Ping1D-id-43-t-1-m-1-v-3.19
description = "/dev/serial/ping/Ping1D-id-%s-t-%s-m-%s-v-%s.%s" % (
device_id["device_id"],
firmware_version["device_type"],
firmware_version["device_model"],
firmware_version["firmware_version_major"],
firmware_version["firmware_version_minor"])
print("Found Ping1D (ID: %d) at %s" % (device_id["device_id"], dev))
# Follow link to actual device
target_device = subprocess.check_output(' '.join(["readlink", "-f", byiddev]), shell=True)
# Strip newline from output
target_device = target_device.split('\n')[0]
# Create another link to it
print("Creating symbolic link to", target_device)
subprocess.check_output(' '.join(["mkdir", "-p", "/dev/serial/ping"]), shell=True)
output = subprocess.check_output("ln -fs %s %s" % (
target_device,
description), shell=True)
pings_found += 1
except subprocess.CalledProcessError as exception:
print(exception)
continue
exit(pings_found)
import time,sys
import os,zmq
sys.path.append('..')
sys.path.append('../utils')
import zmq_wrapper as utils
print('done import 2')
import zmq_topics
import asyncio,pickle
from brping import Ping1D
import detect_usb
dev=detect_usb.devmap['SONAR_USB']
myPing = Ping1D(dev, 115200)
if myPing.initialize() is False:
print("Failed to initialize Ping!")
exit(1)
pub_sonar = utils.publisher(zmq_topics.topic_sonar_port)
cnt=0
while 1:
time.sleep(0.1)
data = myPing.get_distance()
if cnt%10==0:
print('sonar ',data)
tic=time.time()
tosend = pickle.dumps({'ts':tic, 'sonar':[data['distance'],data['confidence']]})
pub_sonar.send_multipart([zmq_topics.topic_sonar,tosend])
cnt+=1
#
def __init__(self, ping_descriptor):
self.ping = Ping1D(ping_descriptor, PING_BAUDRATE)
if self.ping.initialize() is False:
lcd.print("Failed to initialize Ping!")
exit(1)
