def send_gps_nmea_sentence(sentence):
sentence = sentence.strip()
msg = NmeaSentence_msg()
msg.sentence = sentence
msg.header.stamp = rospy.get_rostime()
msg.header.frame_id = 'gps0_link'
gps_pub.publish(msg)
def main():
rospy.init_node('nmea_topic_serial_reader')
nmea_pub = rospy.Publisher("nmea_sentence", Sentence, queue_size=1)
serial_port = rospy.get_param('~port', '/dev/ttyUSB0')
serial_baud = rospy.get_param('~baud', 4800)
# Get the frame_id
frame_id = RosNMEADriver.get_frame_id()
try:
GPS = serial.Serial(port=serial_port, baudrate=serial_baud, timeout=2)
while not rospy.is_shutdown():
data = GPS.readline().strip()
sentence = Sentence()
sentence.header.stamp = rospy.get_rostime()
sentence.header.frame_id = frame_id
sentence.sentence = data
# log the raw nmea (hope we will see some GGA)
rospy.loginfo_throttle(1, sentence)
nmea_pub.publish(sentence)
except rospy.ROSInterruptException:
GPS.close() # Close GPS serial port
def main():
rospy.init_node('nmea_topic_serial_reader')
nmea_pub = rospy.Publisher("nmea_sentence", Sentence, queue_size=1)
serial_port = rospy.get_param('~port', '/dev/ttyUSB0')
serial_baud = rospy.get_param('~baud', 4800)
# Get the frame_id
frame_id = RosNMEADriver.get_frame_id()
try:
GPS = serial.Serial(port=serial_port, baudrate=serial_baud, timeout=2)
while not rospy.is_shutdown():
data = GPS.readline().strip()
sentence = Sentence()
sentence.header.stamp = rospy.get_rostime()
sentence.header.frame_id = frame_id
sentence.sentence = data
nmea_pub.publish(sentence)
except rospy.ROSInterruptException:
GPS.close() # Close GPS serial port
def publish_nmea(self, sentence, frame_id):
msg = Sentence()
msg.header.stamp = rospy.get_rostime()
msg.header.frame_id = frame_id
msg.sentence = sentence
self.pub.publish(msg)
def main(args=None):
rclpy.init(args=args)
driver = Ros2NMEADriver()
nmea_pub = driver.create_publisher(Sentence, "nmea_sentence", 10)
serial_port = driver.declare_parameter('port', '/dev/ttyUSB0').value
serial_baud = driver.declare_parameter('baud', 4800).value
# Get the frame_id
frame_id = driver.get_frame_id()
try:
GPS = serial.Serial(port=serial_port, baudrate=serial_baud, timeout=2)
try:
while rclpy.ok():
data = GPS.readline().strip()
sentence = Sentence()
sentence.header.stamp = driver.get_clock().now().to_msg()
sentence.header.frame_id = frame_id
sentence.sentence = data
nmea_pub.publish(sentence)
except Exception as e:
driver.get_logger().error("Ros error: {0}".format(e))
GPS.close() # Close GPS serial port
except serial.SerialException as ex:
driver.get_logger().fatal(
"Could not open serial port: I/O error({0}): {1}".format(
ex.errno, ex.strerror))
def run(self):
self.setup()
while not rospy.is_shutdown():
packet = self.getPacket()
if packet:
s = Sentence()
s.header.stamp = rospy.Time.now()
s.sentence = packet.params.rstrip()
self.publisher.publish(s)
def ():
pub = rospy.Publisher('GPS_msg', Sentence, queue_size=10)
rospy.init_node('serial_to_ROS', anonymous=True)
rospy.Rate(10)
##do stuff
serial_line = ser.readline() #unsure of how to implement timeout
msg = Sentence()
msg.sentence = serial_line
pub.publish(msg)
def publish_gga(gga, configs):
global nmea_pub
global seq
msg = Sentence()
msg.header.frame_id = configs['gps_origin_frame']
msg.header.seq = seq
seq += 1
msg.header.stamp = rospy.Time.now()
msg.sentence = gga
if javad_delta.GGA.is_good_gga(msg.sentence):
nmea_pub.publish(msg)
return msg
def main():
"""Create and run the nmea_topic_serial_reader ROS node.
Opens a serial device and publishes data from the device as nmea_msgs.msg.Sentence messages.
:ROS Parameters:
- ~port (str)
Path of the serial device to open.
- ~baud (int)
Baud rate to configure the serial device.
:ROS Publishers:
- nmea_sentence (nmea_msgs.msg.Sentence)
Publishes each line from the openserial device as a new message. The header's stamp is
set to the rostime when the data is read from the serial device.
"""
rospy.init_node('nmea_topic_serial_reader')
nmea_pub = rospy.Publisher("nmea_sentence", Sentence, queue_size=1)
serial_port = rospy.get_param('~port', '/dev/ttyUSB0')
serial_baud = rospy.get_param('~baud', 4800)
# Get the frame_id
frame_id = RosNMEADriver.get_frame_id()
try:
GPS = serial.Serial(port=serial_port, baudrate=serial_baud, timeout=2)
while not rospy.is_shutdown():
data = GPS.readline().strip()
sentence = Sentence()
sentence.header.stamp = rospy.get_rostime()
sentence.header.frame_id = frame_id
try:
sentence.sentence = data.decode('ascii')
except UnicodeError as e:
rospy.logwarn(
"Skipped reading a line from the serial device because it could not be "
"decoded as an ASCII string. The bytes were {0}".format(
data))
else:
nmea_pub.publish(sentence)
except rospy.ROSInterruptException:
GPS.close() # Close GPS serial port
def cb_sub(self, msg):
for obs in msg.obstacles:
# generate 2d convex hull, birds-eye view
# if not using convex hull (optional) from obstacle points (which may not exist)
# generate rectangle from centroid + dimensions
# tf transform
if not self.tf_frame is None and len(self.tf_frame) > 0:
self.ft.transform_obstacle(obs, self.tf_frame)
pts = []
if self.convex_hull and not obs.hull2d is None and len(
obs.hull2d.points) > 0:
# hull2d.points are points relative to pose.position, convert to absolute in tf_frame
centroid = obs.pose.pose.position
pts = [(centroid.x + pt.x, centroid.y + pt.y)
for pt in obs.hull2d.points]
if len(
pts
) == 0: # no convex hull points, create hull from obstacle dimensions
l, w = obs.dimensions.x, obs.dimensions.y
x, y = obs.pose.pose.position.x, obs.pose.pose.position.y
pts = [(x - l / 2., y - w / 2.), (x - l / 2., y + w / 2.),
(x + l / 2., y + w / 2.), (x + l / 2., y - w / 2.)]
s = Sentence()
s.header = obs.header
s.header.stamp = rospy.Time.now() # don't preserve timestamp?
# generate hull string fragment
hull_frag = str()
for pt in pts:
hull_frag += ",%f,%f" % (pt[0], pt[1])
# sec.nsec, id, 2d convex hull( x1,y1,x2,y2,...,xn,yn )*00\n
s.sentence = '$PYOBP,%d.%d,%s%s*00\n' % (s.header.stamp.to_sec(),
s.header.stamp.to_nsec(),
obs.id, hull_frag)
self.pub.publish(s)
def __init__(self):
# Generate msgs
self.nmea = Sentence()
self.flag_new_data = False
# Config
self.get_config()
self.TCP_IP = self.config['TCP_IP']
self.TCP_PORT = self.config['TCP_PORT']
self.BUFFER_SIZE = self.config['BUFFER_SIZE']
self.socket_listen = None
def main():
"""Create and run the nmea_topic_serial_reader ROS node.
Opens a serial device and publishes data from the device as nmea_msgs.msg.Sentence messages.
ROS parameters:
~port (str): Path of the serial device to open.
~baud (int): Baud rate to configure the serial device.
ROS publishers:
nmea_sentence (nmea_msgs.msg.Sentence): Publishes each line from the open serial device as a new
message. The header's stamp is set to the rostime when the data is read from the serial device.
"""
rospy.init_node('nmea_topic_serial_reader') #初始化ros节点名
nmea_pub = rospy.Publisher("nmea_sentence", Sentence, queue_size=1) #定义发布者
serial_port = rospy.get_param('~port', '/dev/ttyUSB0') #设置串口设备文件名
serial_baud = rospy.get_param('~baud', 4800) #设置波特率
# Get the frame_id
frame_id = RosNMEADriver.get_frame_id() #设置frame_id
try:
GPS = serial.Serial(port=serial_port, baudrate=serial_baud,
timeout=2) #调用python串口模块Serial
while not rospy.is_shutdown():
data = GPS.readline().strip() #从串口读入数据
sentence = Sentence() #创建nmea_msgs/Sentence结构变量sentence
sentence.header.stamp = rospy.get_rostime() #设置sentence的时间戳，为当前时间
sentence.header.frame_id = frame_id #设置sentence的frame_id
sentence.sentence = data #设置sentence的sentence变量。
nmea_pub.publish(sentence) #发布sentence
except rospy.ROSInterruptException: #异常处理
GPS.close() # Close GPS serial port
def main():
"""Create and run the nmea_topic_serial_reader ROS node.
Opens a serial device and publishes data from the device as nmea_msgs.msg.Sentence messages.
ROS parameters:
~port (str): Path of the serial device to open.
~baud (int): Baud rate to configure the serial device.
ROS publishers:
nmea_sentence (nmea_msgs.msg.Sentence): Publishes each line from the open serial device as a new
message. The header's stamp is set to the rostime when the data is read from the serial device.
"""
rospy.init_node('nmea_topic_serial_reader')
nmea_pub = rospy.Publisher("nmea_sentence", Sentence, queue_size=1)
serial_port = rospy.get_param('~port', '/dev/ttyUSB0')
serial_baud = rospy.get_param('~baud', 4800)
gps_frame = rospy.get_param('~frame_id', 'gps')
# Get the frame_id
# frame_id = RosNMEADriver.get_frame_id()
try:
GPS = serial.Serial(port=serial_port, baudrate=serial_baud, timeout=2)
while not rospy.is_shutdown():
data = GPS.readline().strip()
sentence = Sentence()
sentence.header.stamp = rospy.get_rostime()
sentence.header.frame_id = gps_frame
sentence.sentence = data
nmea_pub.publish(sentence)
except rospy.ROSInterruptException:
GPS.close() # Close GPS serial port
def tx(self, *fields):
if not hasattr(self, 'tx_publisher'):
self.tx_publisher = rospy.Publisher('tx', Sentence, queue_size=1)
def process_field(val):
if isinstance(val, str):
return val
if isinstance(val, int):
return str(val)
if isinstance(val, float):
return (self.NMEA_FLOAT_FORMAT % val).rstrip('0').rstrip('.')
return str(val)
fields = map(process_field, fields)
sentence_body = "%s%s,%s" % (self.TALKER, self.SENTENCE,
",".join(fields))
s = Sentence(sentence="$%s*%s" %
(sentence_body, checksum(sentence_body)))
s.header.stamp = rospy.Time.now()
self.tx_publisher.publish(s)
def sendSentence(self, msg):
sentence = Sentence()
sentence.header.stamp = rospy.Time.now()
sentence.sentence = "$" + msg + "*" + checksum(msg)
self.pub_nmea.publish(sentence)
import rospy
from nmea_msgs.msg import Sentence
from libnmea_navsat_driver.driver import RosNMEADriver
if __name__ == '__main__':
rospy.init_node('nmea_topic_serial_reader')
nmea_pub = rospy.Publisher("nmea_sentence", Sentence, queue_size=1)
serial_port = rospy.get_param('~port','/dev/ttyUSB0')
serial_baud = rospy.get_param('~baud',4800)
# Get the frame_id
frame_id = RosNMEADriver.get_frame_id()
try:
GPS = serial.Serial(port=serial_port, baudrate=serial_baud, timeout=2)
while not rospy.is_shutdown():
data = GPS.readline().strip()
sentence = Sentence()
sentence.header.stamp = rospy.get_rostime()
sentence.header.frame_id = frame_id
sentence.sentence = data
nmea_pub.publish(sentence)
except rospy.ROSInterruptException:
GPS.close() #Close GPS serial port
def sub_generateNMEA_callback(self, data):
''' Service message type is
@param geometry_msgs/PostStamped Pose
Returns
nmea_msgs/Sentence GPS_raw
'''
#rospy.logdebug(data)
pose = data.PoseStamped.pose
hdr = data.PoseStamped.header
# Get time stamp from sent data (or from PC)
dt = datetime.datetime.utcfromtimestamp(hdr.stamp.secs +
hdr.stamp.nsecs / 1.0e9)
if self.UTM_origin is None:
rospy.logerr('UTM Origin is not set.')
return None
# Add local coordinate frame to UTM origin.
# The assumption here is that navigation is done in a local reference frame
# in Eastings and Northings from UTM_origin. This keeps numeric values
# small and prevents confounding precision errors.
# Convert UTM to LAT/LON
z, b = self.UTM_origin.gridZone()
easting_to_convert = self.UTM_origin.easting + pose.position.x
northing_to_convert = self.UTM_origin.northing + pose.position.y
rospy.logdebug(
"Got %s and %s, UTMOrigin is %0.2f,%0.2f,%s, Converting %0.2f, %0.2f"
% (pose.position.x, pose.position.y, self.UTM_origin.easting,
self.UTM_origin.northing, str(z) + b, easting_to_convert,
northing_to_convert))
p = geodesy.utm.UTMPoint(easting_to_convert,
northing_to_convert,
pose.position.z,
zone=z,
band=b)
if p.valid:
pLL = p.toMsg()
else:
rospy.logerr('Error converting UTM position to Lat/Lon')
return None
# Format timestamp for NMEA string
timestr = dt.strftime('%H%M%S.%f')
# Format LAT/LON for NMEA string.
latstr, lonstr = self.format_latlon_for_NMEA(pLL.latitude,
pLL.longitude)
# Assemble faked NMEA string
nmea_str = ('$GPGGA,' + timestr + ',' + latstr + ',' + lonstr + ',' +
'08,0.9,1.0,M,32.0,M,1,0,*')
G = Sentence()
G.sentence = nmea_str + self.checksum(nmea_str)
#G.header.seq = 1
#G.header.frame_id = 1
#unixtime = (dt - datetime.datetime.utcfromtimestamp(0)).total_seconds()
#G.header.stamp.secs = int(unixtime)
#G.header.stamp.nsecs = int( (unixtime - math.floor(unixtime)) * 1e9 )
rospy.loginfo('SENDING: ' + G.sentence)
return G
#!/usr/bin/env python
#Import ROS stuff
import rospy
from nmea_msgs.msg import Sentence
#Import socket for getting data from android
import socket
#Server setup for recieving GPS data
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
port = 6000
client_address = ('192.168.43.10', port)
#Init rospy stuff
rospy.init_node("gps_publisher.py", anonymous=True)
pub = rospy.Publisher('/nmea_sentence', Sentence, queue_size=10)
s = Sentence()
try:
sock.connect(client_address)
while True:
data = sock.recv(10000000)
if data[0] == 'G':
s.sentence = '$' + data[0:-2]
s.header.stamp = rospy.Time.now()
s.header.frame_id = 'gps'
pub.publish(s)
finally:
sock.close()
"$GPGSV,2,2,06,27,48,050,39,30,35,308,33*77",
"$GLGSV,2,1,06,70,12,282,31,71,10,330,24,79,66,063,39,80,41,321,35*65",
"$GLGSV,2,2,06,81,66,074,37,88,24,031,31*6B",
"$GNGLL,4243.80095,N,07340.75456,W,183829.00,A,A*6C",
"$GNRMC,183830.00,A,4243.80100,N,07340.75457,W,0.043,,100419,,,A*7B",
"$GNVTG,,T,,M,0.043,N,0.079,K,A*34",
"$GNGGA,183830.00,4243.80100,N,07340.75457,W,1,07,6.70,76.8,M,-33.7,M,,*48",
"$GNGSA,A,3,07,27,30,,,,,,,,,,10.63,6.70,8.25*27",
"$GNGSA,A,3,81,79,88,80,,,,,,,,,10.63,6.70,8.25*29",
"$GPGSV,2,1,06,04,,,31,07,70,297,36,08,,,24,23,12,197,23*71",
"$GPGSV,2,2,06,27,48,050,38,30,35,308,33*76",
"$GLGSV,2,1,06,70,12,282,32,71,10,330,24,79,66,063,37,80,41,321,34*69]"
]
if __name__ == "__main__":
rospy.init_node('publish_static_gps')
gps_pub = rospy.Publisher('/gps/nmea_sentence',
NmeaSentence_msg,
queue_size=2)
while not rospy.is_shutdown():
for sentence in gps_data:
sentence = sentence.strip()
msg = NmeaSentence_msg()
msg.sentence = sentence
msg.header.stamp = rospy.get_rostime()
msg.header.frame_id = 'gps0_link'
gps_pub.publish(msg)
time.sleep(0.1)
# main(imu_pub, mag_pub)
def nmea_depth_tcp():
# Init node
system_name = socket.gethostname()
rospy.init_node("lowrance_sonar", anonymous=True)
rospy.loginfo("[nmea_depth_tcp] Initializing node...")
# Parameters
tcp_addr = rospy.get_param('~address')
tcp_port = rospy.get_param('~port', 10110) # Lowrance standard port
update_rate = rospy.get_param(
'~update_rate', 40) # Measurement comm rate for Lowrance (Hz)
# Connect TCP client to destination
try:
tcp_in.connect((tcp_addr, tcp_port))
except IOError as exp:
rospy.logerr("Socket error: %s" % exp.strerror)
rospy.signal_shutdown(reason="Socket error: %s" % exp.strerror)
# NMEA Sentence publisher (to publish NMEA sentence regardless of content)
sentence_pub = rospy.Publisher("%s/sonar/nmea_sentence" % system_name,
Sentence,
queue_size=10)
# GPS publishers
# GPGGA - Position
# GPVTG - Velocity Track Good (ground speed)
# GPZDA - Time reference (GPST)
# GPGSA - Active Satellites
# GPGSV - Satellites in View
position_pub = rospy.Publisher("%s/sonar/gps/fix" % system_name,
NavSatFix,
queue_size=10)
vel_pub = rospy.Publisher("%s/sonar/gps/vel" % system_name,
TwistStamped,
queue_size=10)
timeref_pub = rospy.Publisher("%s/sonar/gps/time_reference" % system_name,
TimeReference,
queue_size=10)
active_sat_pub = rospy.Publisher("%s/sonar/gps/active_satellites" %
system_name,
Gpgsa,
queue_size=10)
sat_view_pub = rospy.Publisher("%s/sonar/gps/satellites_in_view" %
system_name,
Gpgsv,
queue_size=10)
# Sidescanner publishers
# SDDBT - Depth Below Transducer
# SDDPT - Depth of Water
# SDMTW - Mean Temperature of Water
# SDVHW - Velocity and heading in Water
# SDHDG - Magnetic heading
depth_below_trans_pub = rospy.Publisher(
"%s/sonar/scanner/water/depth_below_transducer" % system_name,
DepthBelowTransducer,
queue_size=10)
depth_water_pub = rospy.Publisher("%s/sonar/scanner/water/depth" %
system_name,
DepthOfWater,
queue_size=10)
temp_water_pub = rospy.Publisher("%s/sonar/scanner/water/temperature" %
system_name,
Temperature,
queue_size=10)
water_heading_speed_pub = rospy.Publisher(
"%s/sonar/scanner/water/heading_and_speed" % system_name,
WaterHeadingSpeed,
queue_size=10)
mag_heading_pub = rospy.Publisher("%s/sonar/scanner/magnetic_heading" %
system_name,
MagneticHeading,
queue_size=10)
# Diagnostics publisher
diag_pub = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size=10)
rate = rospy.Rate(update_rate) # Defines the publish rate
rospy.loginfo("[nmea_depth_tcp] Initialization done.")
# rospy.loginfo("[nmea_depth_tcp] Published topics:")
# rospy.loginfo("[nmea_depth_tcp] Sentence:\t\t\t%s/nmea_sentence" % system_name)
# rospy.loginfo("[nmea_depth_tcp] GPS Fix:\t\t\t%s/fix" % system_name)
# rospy.loginfo("[nmea_depth_tcp] GPS Velocity:\t\t%s/vel" % system_name)
# rospy.loginfo("[nmea_depth_tcp] Time Reference:\t\t%s/time_reference" % system_name)
# rospy.loginfo("[nmea_depth_tcp] Depth of Water:\t\t%s/depth/water" % system_name)
# rospy.loginfo("[nmea_depth_tcp] Depth below transducer:\t%s/depth/below_transducer" % system_name)
# Run node
last_update = 0
while not rospy.is_shutdown():
try:
nmea_in = tcp_in.makefile().readline()
except socket.error:
pass
nmea_parts = nmea_in.strip().split(',')
ros_now = rospy.Time().now()
diag_msg = DiagnosticArray()
diag_msg.status.append(DiagnosticStatus())
diag_msg.status[0].name = 'sonar'
diag_msg.status[0].hardware_id = '%s' % system_name
if len(nmea_parts):
#### GPS
# GPS Fix position
if nmea_parts[0] == '$GPGGA' and len(nmea_parts) >= 10:
latitude = cast_float(
nmea_parts[2][0:2]) + cast_float(nmea_parts[2][2:]) / 60.0
if nmea_parts[3] == 'S':
latitude = -latitude
longitude = cast_float(
nmea_parts[4][0:3]) + cast_float(nmea_parts[4][3:]) / 60.0
if nmea_parts[5] == 'W':
longitude = -longitude
altitude = cast_float(nmea_parts[9])
nsf = NavSatFix()
nsf.header.stamp = ros_now
nsf.header.frame_id = system_name
nsf.latitude = latitude
nsf.longitude = longitude
nsf.altitude = altitude
position_pub.publish(nsf)
# Velocity
if nmea_parts[0] == '$GPVTG' and len(nmea_parts) >= 9:
vel = TwistStamped()
vel.header.frame_id = system_name
vel.header.stamp = ros_now
vel.twist.linear.x = cast_float(
nmea_parts[7]) / 3.6 # Km/h to m/s
vel_pub.publish(vel)
# Time reference (GPST)
if nmea_parts[0] == '$GPZDA' and len(nmea_parts) >= 5:
tref = TimeReference()
tref.header.frame_id = system_name
tref.header.stamp = ros_now
hour = cast_int(nmea_parts[1][0:2])
minute = cast_int(nmea_parts[1][2:4])
second = cast_int(nmea_parts[1][4:6])
try:
ms = int(float(nmea_parts[1][6:]) * 1000000)
except ValueError:
ms = 0
day = cast_int(nmea_parts[2])
month = cast_int(nmea_parts[3])
year = cast_int(nmea_parts[4])
try:
zda = datetime.datetime(year, month, day, hour, minute,
second, ms)
tref.time_ref = rospy.Time(
calendar.timegm(zda.timetuple()),
zda.microsecond * 1000)
except ValueError:
pass
tref.source = system_name
timeref_pub.publish(tref)
# GPS DOP and active satellites
if nmea_parts[0] == '$GPGSA' and len(nmea_parts) >= 18:
gsa = Gpgsa()
gsa.header.frame_id = system_name
gsa.header.stamp = ros_now
gsa.auto_manual_mode = nmea_parts[1]
gsa.fix_mode = cast_int(nmea_parts[2])
satellite_list = []
for x in nmea_parts[3:14]:
try:
satellite_list.append(int(x))
except ValueError:
break
gsa.sv_ids = satellite_list
gsa.pdop = cast_float(nmea_parts[15])
gsa.hdop = cast_float(nmea_parts[16])
gsa.vdop = cast_float(nmea_parts[17])
active_sat_pub.publish(gsa)
# GPS Satellites in View
if nmea_parts[0] == '$GPGSV' and len(nmea_parts) >= 7:
gsv = Gpgsv()
gsv.header.frame_id = system_name
gsv.header.stamp = ros_now
gsv.n_msgs = cast_int(nmea_parts[1])
gsv.msg_number = cast_int(nmea_parts[2])
gsv.n_satellites = cast_int(nmea_parts[3])
for i in range(4, len(nmea_parts), 4):
gsv_sat = GpgsvSatellite()
try:
gsv_sat.prn = int(nmea_parts[i])
except ValueError:
pass
try:
gsv_sat.elevation = int(nmea_parts[i + 1])
except ValueError:
pass
try:
gsv_sat.azimuth = int(nmea_parts[i + 2])
except ValueError:
pass
try:
gsv_sat.snr = int(nmea_parts[i + 3].split("*")[0])
except ValueError:
pass
gsv.satellites.append(gsv_sat)
sat_view_pub.publish(gsv)
#### Side-scanner
# Depth (DBT - Depth Below Transducer)
if nmea_parts[0] == '$SDDBT' and len(nmea_parts) >= 7:
d = DepthBelowTransducer()
d.header.frame_id = system_name
d.header.stamp = ros_now
try:
d.feet = cast_float(nmea_parts[1]) # In feet
except ValueError:
pass
try:
d.meters = cast_float(nmea_parts[3]) # In meters
except ValueError:
pass
try:
d.fathoms = cast_float(nmea_parts[5]) # In fathoms
except ValueError:
pass
depth_below_trans_pub.publish(d)
# Depth (DPT - DePTh of water)
if nmea_parts[0] == '$SDDPT' and len(nmea_parts) >= 4:
d = DepthOfWater()
d.header.frame_id = system_name
d.header.stamp = ros_now
try:
d.depth = cast_float(nmea_parts[1]) # In meters
except ValueError:
pass
try:
d.offset = cast_float(nmea_parts[2]) # In meters
except ValueError:
pass
try:
d.range = cast_float(nmea_parts[3])
except ValueError:
pass
depth_water_pub.publish(d)
# SDMTW - Mean Temperature of Water
if nmea_parts[0] == '$SDMTW' and len(nmea_parts) >= 3:
tempC = Temperature()
tempC.header.frame_id = system_name
tempC.header.stamp = ros_now
tempC.temperature = cast_float(nmea_parts[1])
temp_water_pub.publish(tempC)
# SDVHW - Water Heading and Speed
if nmea_parts[0] == '$SDVHW' and len(nmea_parts) >= 9:
whs = WaterHeadingSpeed()
whs.header.frame_id = system_name
whs.header.stamp = ros_now
whs.true_heading = cast_float(nmea_parts[1])
whs.mag_heading = cast_float(nmea_parts[3])
whs.knots = cast_float(nmea_parts[5])
whs.kmph = cast_float(nmea_parts[7])
whs.mps = whs.kmph / 3.6 # Km/h to m/s
water_heading_speed_pub.publish(whs)
# SDHDG - Magnetic heading
if nmea_parts[0] == '$SDHDG' and len(nmea_parts) >= 6:
hdg = MagneticHeading()
hdg.header.frame_id = system_name
hdg.header.stamp = ros_now
hdg.heading = cast_float(nmea_parts[1])
hdg.mag_dev = cast_float(nmea_parts[2])
hdg.mag_dev_dir = nmea_parts[3]
hdg.mag_var = cast_float(nmea_parts[4])
hdg.mag_var_dir = nmea_parts[5].split('*')[0]
quat = quaternion_from_euler(0.0, 0.0, cast_float(hdg.heading))
hdg.quaternion.x = quat[0]
hdg.quaternion.y = quat[1]
hdg.quaternion.z = quat[2]
hdg.quaternion.w = quat[3]
mag_heading_pub.publish(hdg)
# NMEA Sentence (published regardless of content)
sentence_msg = Sentence()
sentence_msg.header.frame_id = system_name
sentence_msg.header.stamp = ros_now
sentence_msg.sentence = nmea_in
sentence_pub.publish(sentence_msg)
diag_msg.status[0].level = DiagnosticStatus.OK
diag_msg.status[0].message = 'OK'
diag_msg.status[0].values = [
KeyValue(key="Sentence", value=sentence_msg.sentence)
]
last_update = ros_now
# Check for stale status
elapsed = ros_now.to_sec() - last_update.to_sec()
if elapsed > 35:
diag_msg.status[0].level = DiagnosticStatus.STALE
diag_msg.status[0].message = 'Stale'
diag_msg.status[0].values = [
KeyValue(key="Update Status", value='Stale'),
KeyValue(key="Time Since Update", value=str(elapsed))
]
# Publish diagnostics message
diag_pub.publish(diag_msg)
# Node sleeps for some time
rate.sleep()
def broadcast_gps(self, gps):
message = Sentence()
message.header.frame_id = "gps"
message.header.stamp = rospy.get_rostime()
message.sentence = gps
self.sentence_publisher.publish(message)
odom.twist.covariance = [0 for i in range(36)]
odom.twist.covariance[0] = 0
odom.twist.covariance[7] = 0
quat = [0, 0, 0, 0]
quat_vth = [0, 0, 0, 0]
odom_trans = TransformStamped()
br = tf.TransformBroadcaster()
imu_pub = rospy.Publisher("imu_data", Imu, queue_size=50)
imu = Imu()
imu.header.frame_id = "chassis"
nmea_pub = rospy.Publisher("nmea_sentence", Sentence, queue_size=10)
nmea_msg = Sentence()
nmea_msg.header.frame_id = 'chassis'
x = 0.0
y = 0.0
th = 0.0
theta = 0
dummy_vth = 0
wheel_circumference = 0.352 #d=0.112 #0.4 #d=0.1274 #0.3947 #d = 0.1257 #0.4584 #(sprocket + track height) diameter = 0.146 #0.12 #0.136
axel_length = 0.398
offset_r = 0
offset_p = 0
offset_y = 0
def nmea_depth_udp():
# Init node
rospy.init_node("nmea_depth_udp", anonymous=True)
rospy.loginfo("[nmea_depth_udp] Initializing node...")
rate = rospy.Rate(10) # 10 Hz
# Parameters
udp_addr = rospy.get_param('~address', '')
udp_port = rospy.get_param('~port', 12021) # Random palindrome port
device_frame_id = rospy.get_param('~frame_id', "")
# Start UDP socket (defaults to any IP and port 12021)
udp_in.bind((udp_addr, udp_port))
# Publishers
sentence_pub = rospy.Publisher("%s/nmea_sentence" % device_frame_id,
Sentence,
queue_size=10)
position_pub = rospy.Publisher("%s/fix" % device_frame_id,
NavSatFix,
queue_size=10)
vel_pub = rospy.Publisher("%s/vel" % device_frame_id,
TwistStamped,
queue_size=10)
timeref_pub = rospy.Publisher("%s/time_reference" % device_frame_id,
TimeReference,
queue_size=10)
depth_below_trans_pub = rospy.Publisher("%s/depth/below_transducer" %
device_frame_id,
DepthBelowTransducer,
queue_size=10)
depth_water_pub = rospy.Publisher("%s/depth/water" % device_frame_id,
DepthOfWater,
queue_size=10)
rospy.loginfo("[nmea_depth_udp] Initialization done.")
rospy.loginfo("[nmea_depth_udp] Published topics:")
rospy.loginfo("[nmea_depth_udp] Sentence:\t\t\t%s/nmea_sentence" %
device_frame_id)
rospy.loginfo("[nmea_depth_udp] GPS Fix:\t\t\t%s/fix" % device_frame_id)
rospy.loginfo("[nmea_depth_udp] GPS Velocity:\t\t%s/vel" % device_frame_id)
rospy.loginfo("[nmea_depth_udp] Time Reference:\t\t%s/time_reference" %
device_frame_id)
rospy.loginfo("[nmea_depth_udp] Depth of Water:\t\t%s/depth/water" %
device_frame_id)
rospy.loginfo(
"[nmea_depth_udp] Depth below transducer:\t%s/depth/below_transducer" %
device_frame_id)
# Run node
while not rospy.is_shutdown():
try:
nmea_in, _ = udp_in.recvfrom(1024)
except socket.error:
pass
ros_now = rospy.Time().now()
nmea_parts = nmea_in.strip().split(',')
if len(nmea_parts):
try:
# GPS Fix position
if nmea_parts[0] == '$GPGGA' and len(nmea_parts) >= 10:
latitude = int(
nmea_parts[2][0:2]) + float(nmea_parts[2][2:]) / 60.0
if nmea_parts[3] == 'S':
latitude = -latitude
longitude = int(
nmea_parts[4][0:3]) + float(nmea_parts[4][3:]) / 60.0
if nmea_parts[5] == 'W':
longitude = -longitude
# altitude = float(nmea_parts[9])
nsf = NavSatFix()
nsf.header.stamp = ros_now
nsf.header.frame_id = device_frame_id
nsf.latitude = latitude
nsf.longitude = longitude
# nsf.altitude = altitude
position_pub.publish(nsf)
# Velocity
if nmea_parts[0] == '$GPVTG' and len(nmea_parts) >= 9:
vel = TwistStamped()
vel.header.frame_id = device_frame_id
vel.header.stamp = ros_now
vel.twist.linear.x = float(
nmea_parts[7]) / 3.6 # Km/h to m/s
vel_pub.publish(vel)
# Time reference (GPST)
if nmea_parts[0] == '$GPZDA' and len(nmea_parts) >= 5:
tref = TimeReference()
tref.header.frame_id = device_frame_id
tref.header.stamp = ros_now
hour = int(nmea_parts[1][0:2])
minute = int(nmea_parts[1][2:4])
second = int(nmea_parts[1][4:6])
try:
ms = int(float(nmea_parts[1][6:]) * 1000000)
except ValueError:
ms = 0
day = int(nmea_parts[2])
month = int(nmea_parts[3])
year = int(nmea_parts[4])
zda = datetime.datetime(year, month, day, hour, minute,
second, ms)
tref.time_ref = rospy.Time(
calendar.timegm(zda.timetuple()),
zda.microsecond * 1000)
tref.source = device_frame_id
timeref_pub.publish(tref)
# Depth (DBT - Depth Below Transducer)
if nmea_parts[0] == '$SDDBT' and len(nmea_parts) >= 7:
d = DepthBelowTransducer()
d.header.frame_id = device_frame_id
d.header.stamp = ros_now
try:
d.feet = float(nmea_parts[1]) # In feet
except ValueError:
pass
try:
d.meters = float(nmea_parts[3]) # In meters
except ValueError:
pass
try:
d.fathoms = float(nmea_parts[5]) # In fathoms
except ValueError:
pass
depth_below_trans_pub.publish(d)
# Depth (DPT - DePTh of water)
if nmea_parts[0] == '$SDDPT' and len(nmea_parts) >= 4:
d = DepthOfWater()
d.header.frame_id = device_frame_id
d.header.stamp = ros_now
try:
d.depth = float(nmea_parts[1]) # In meters
except ValueError:
pass
try:
d.offset = float(nmea_parts[2]) # In meters
except ValueError:
pass
try:
d.range = float(nmea_parts[3])
except ValueError:
pass
depth_water_pub.publish(d)
#### Other possible parsings (from Heck's provided logs)
# GPGLL - Geographic Latitude and Longitude (legacy sentence, same info as contained in GPGGA)
# GPGSA - Gps dillution of position and active SAtellites
# GPGSV - Gps Satellites in View
# GPRMC - Recommendec Minimum navigation type C (includes latitude, longitude, speed in knots, date... all info already available on other messages)
# SDMTW - Mean Temperature of Water
# SDVHW - Velocity and Heading in Water (Water speed in knots/kilometers-per-hour and heading in magnetic degrees)
# SDHDG - magnetic HeaDinG (in degrees, with deviation and variation)
except ValueError:
pass
# NMEA Sentence (published regardless of content)
sentence_msg = Sentence()
sentence_msg.header.frame_id = device_frame_id
sentence_msg.header.stamp = ros_now
sentence_msg.sentence = nmea_in
sentence_pub.publish(sentence_msg)
# Node sleeps for 10 Hz
rate.sleep()
# publish pressure
elif sensor == 'press_0':
msg_press = Float64()
msg_press.data = float(fields[4])
pub_pressure.publish(msg_press)
# publish light
elif sensor == "light_0":
msg_light = Float64()
msg_light.data = float(fields[4])
pub_light.publish(msg_light)
# publish NMEA setence
elif sensor == "nmea":
msg_nmea = NmeaSentence()
msg_nmea.header.frame_id = 'android'
msg_nmea.header.stamp = rospy.Time.from_sec(float(fields[2]) / 1000)
msg_nmea.sentence = fields[3]
pub_nmea.publish(msg_nmea)
# publish fix and vel from device location
# check https://github.com/mrwojtek/sens-rec/blob/b0d34aad1e827720cb6eb11b512ad9eff021a1e5/lib/src/main/java/pl/mrwojtek/sensrec/LocationRecorder.java#L113
# check https://developer.android.com/reference/android/location/Location.html#getAccuracy()
# gps 48021735 52.51782937923822 13.447371576740874 82.6810805981322 0.0 9.953303E-4 12.0 1496696180000
# type ms lat lon alt bearing[deg] speed[m/s] accuracy[m] time[s]
# horizontal accuracy of this location, radial, in meters
# bearing is the horizontal direction of travel of this device (0.0, 360.0], 0.0 = no bearing [deg]
elif sensor == 'gps':
stamp = rospy.Time.from_sec(float(fields[8]) / 1000)
def nmea_depth_udp():
# Init node
rospy.init_node("nmea_depth_udp", anonymous=True)
rospy.loginfo("[nmea_depth_udp] Initializing node...")
rate = rospy.Rate(10) # 10 Hz
# Parameters
udp_addr = rospy.get_param('~address', '')
udp_port = rospy.get_param('~port', 12021) # Random palindrome port
device_frame_id = rospy.get_param('~frame_id', "")
# Start UDP socket (defaults to any IP and port 12021)
udp_in.bind((udp_addr, udp_port))
# NMEA Sentence publisher (to publish NMEA sentence regardless of content)
sentence_pub = rospy.Publisher("%s/nmea_sentence" % device_frame_id,
Sentence,
queue_size=10)
# GPS publishers
# GPGGA - Position
# GPVTG - Velocity Track Good (ground speed)
# GPZDA - Time reference (GPST)
# GPGSA - Active Satellites
# GPGSV - Satellites in View
position_pub = rospy.Publisher("%s/gps/fix" % device_frame_id,
NavSatFix,
queue_size=10)
vel_pub = rospy.Publisher("%s/gps/vel" % device_frame_id,
TwistStamped,
queue_size=10)
timeref_pub = rospy.Publisher("%s/gps/time_reference" % device_frame_id,
TimeReference,
queue_size=10)
active_sat_pub = rospy.Publisher("%s/gps/active_satellites" %
device_frame_id,
Gpgsa,
queue_size=10)
sat_view_pub = rospy.Publisher("%s/gps/satellites_in_view" %
device_frame_id,
Gpgsv,
queue_size=10)
# Sidescanner publishers
# SDDBT - Depth Below Transducer
# SDDPT - Depth of Water
# SDMTW - Mean Temperature of Water
# SDVHW - Velocity and heading in Water
# SDHDG - Magnetic heading
depth_below_trans_pub = rospy.Publisher(
"%s/scanner/water/depth_below_transducer" % device_frame_id,
DepthBelowTransducer,
queue_size=10)
depth_water_pub = rospy.Publisher("%s/scanner/water/depth" %
device_frame_id,
DepthOfWater,
queue_size=10)
temp_water_pub = rospy.Publisher("%s/scanner/water/temperature" %
device_frame_id,
Temperature,
queue_size=10)
water_heading_speed_pub = rospy.Publisher(
"%s/scanner/water/heading_and_speed" % device_frame_id,
WaterHeadingSpeed,
queue_size=10)
mag_heading_pub = rospy.Publisher("%s/scanner/magnetic_heading" %
device_frame_id,
MagneticHeading,
queue_size=10)
rospy.loginfo("[nmea_depth_udp] Initialization done.")
# rospy.loginfo("[nmea_depth_udp] Published topics:")
# rospy.loginfo("[nmea_depth_udp] Sentence:\t\t\t%s/nmea_sentence" % device_frame_id)
# rospy.loginfo("[nmea_depth_udp] GPS Fix:\t\t\t%s/fix" % device_frame_id)
# rospy.loginfo("[nmea_depth_udp] GPS Velocity:\t\t%s/vel" % device_frame_id)
# rospy.loginfo("[nmea_depth_udp] Time Reference:\t\t%s/time_reference" % device_frame_id)
# rospy.loginfo("[nmea_depth_udp] Depth of Water:\t\t%s/depth/water" % device_frame_id)
# rospy.loginfo("[nmea_depth_udp] Depth below transducer:\t%s/depth/below_transducer" % device_frame_id)
# Run node
while not rospy.is_shutdown():
try:
nmea_in, _ = udp_in.recvfrom(1024)
except socket.error:
pass
ros_now = rospy.Time().now()
nmea_parts = nmea_in.strip().split(',')
if len(nmea_parts):
#### GPS
# GPS Fix position
if nmea_parts[0] == '$GPGGA' and len(nmea_parts) >= 10:
latitude = cast_float(
nmea_parts[2][0:2]) + cast_float(nmea_parts[2][2:]) / 60.0
if nmea_parts[3] == 'S':
latitude = -latitude
longitude = cast_float(
nmea_parts[4][0:3]) + cast_float(nmea_parts[4][3:]) / 60.0
if nmea_parts[5] == 'W':
longitude = -longitude
altitude = cast_float(nmea_parts[9])
nsf = NavSatFix()
nsf.header.stamp = ros_now
nsf.header.frame_id = device_frame_id
nsf.latitude = latitude
nsf.longitude = longitude
nsf.altitude = altitude
position_pub.publish(nsf)
# Velocity
if nmea_parts[0] == '$GPVTG' and len(nmea_parts) >= 9:
vel = TwistStamped()
vel.header.frame_id = device_frame_id
vel.header.stamp = ros_now
vel.twist.linear.x = cast_float(
nmea_parts[7]) / 3.6 # Km/h to m/s
vel_pub.publish(vel)
# Time reference (GPST)
if nmea_parts[0] == '$GPZDA' and len(nmea_parts) >= 5:
tref = TimeReference()
tref.header.frame_id = device_frame_id
tref.header.stamp = ros_now
hour = cast_int(nmea_parts[1][0:2])
minute = cast_int(nmea_parts[1][2:4])
second = cast_int(nmea_parts[1][4:6])
try:
ms = int(float(nmea_parts[1][6:]) * 1000000)
except ValueError:
ms = 0
day = cast_int(nmea_parts[2])
month = cast_int(nmea_parts[3])
year = cast_int(nmea_parts[4])
try:
zda = datetime.datetime(year, month, day, hour, minute,
second, ms)
tref.time_ref = rospy.Time(
calendar.timegm(zda.timetuple()),
zda.microsecond * 1000)
except ValueError:
pass
tref.source = device_frame_id
timeref_pub.publish(tref)
# GPS DOP and active satellites
if nmea_parts[0] == '$GPGSA' and len(nmea_parts) >= 18:
gsa = Gpgsa()
gsa.header.frame_id = device_frame_id
gsa.header.stamp = ros_now
gsa.auto_manual_mode = nmea_parts[1]
gsa.fix_mode = cast_int(nmea_parts[2])
satellite_list = []
for x in nmea_parts[3:14]:
try:
satellite_list.append(int(x))
except ValueError:
break
gsa.sv_ids = satellite_list
gsa.pdop = cast_float(nmea_parts[15])
gsa.hdop = cast_float(nmea_parts[16])
gsa.vdop = cast_float(nmea_parts[17])
active_sat_pub.publish(gsa)
# GPS Satellites in View
if nmea_parts[0] == '$GPGSV' and len(nmea_parts) >= 7:
# Typically GPGSV messages come in sequences, run and obtain messages from UDP until last message in sequence arrives
gsv = Gpgsv()
gsv.header.frame_id = device_frame_id
gsv.header.stamp = ros_now
gsv.n_msgs = cast_int(nmea_parts[1])
gsv.msg_number = cast_int(nmea_parts[2])
gsv.n_satellites = cast_int(nmea_parts[3])
for i in range(4, len(nmea_parts), 4):
gsv_sat = GpgsvSatellite()
try:
gsv_sat.prn = int(nmea_parts[i])
except ValueError:
pass
try:
gsv_sat.elevation = int(nmea_parts[i + 1])
except ValueError:
pass
try:
gsv_sat.azimuth = int(nmea_parts[i + 2])
except ValueError:
pass
try:
gsv_sat.snr = int(nmea_parts[i + 3].split("*")[0])
except ValueError:
pass
gsv.satellites.append(gsv_sat)
sat_view_pub.publish(gsv)
#### Side-scanner
# Depth (DBT - Depth Below Transducer)
if nmea_parts[0] == '$SDDBT' and len(nmea_parts) >= 7:
d = DepthBelowTransducer()
d.header.frame_id = device_frame_id
d.header.stamp = ros_now
try:
d.feet = cast_float(nmea_parts[1]) # In feet
except ValueError:
pass
try:
d.meters = cast_float(nmea_parts[3]) # In meters
except ValueError:
pass
try:
d.fathoms = cast_float(nmea_parts[5]) # In fathoms
except ValueError:
pass
depth_below_trans_pub.publish(d)
# Depth (DPT - DePTh of water)
if nmea_parts[0] == '$SDDPT' and len(nmea_parts) >= 4:
d = DepthOfWater()
d.header.frame_id = device_frame_id
d.header.stamp = ros_now
try:
d.depth = cast_float(nmea_parts[1]) # In meters
except ValueError:
pass
try:
d.offset = cast_float(nmea_parts[2]) # In meters
except ValueError:
pass
try:
d.range = cast_float(nmea_parts[3])
except ValueError:
pass
depth_water_pub.publish(d)
# SDMTW - Mean Temperature of Water
if nmea_parts[0] == '$SDMTW' and len(nmea_parts) >= 3:
tempC = Temperature()
tempC.header.frame_id = device_frame_id
tempC.header.stamp = ros_now
tempC.temperature = cast_float(nmea_parts[1])
temp_water_pub.publish(tempC)
# SDVHW - Water Heading and Speed
if nmea_parts[0] == '$SDVHW' and len(nmea_parts) >= 9:
whs = WaterHeadingSpeed()
whs.header.frame_id = device_frame_id
whs.header.stamp = ros_now
whs.true_heading = cast_float(nmea_parts[1])
whs.mag_heading = cast_float(nmea_parts[3])
whs.knots = cast_float(nmea_parts[5])
whs.kmph = cast_float(nmea_parts[7])
whs.mps = whs.kmph / 3.6 # Km/h to m/s
water_heading_speed_pub.publish(whs)
# SDHDG - Magnetic heading
if nmea_parts[0] == '$SDHDG' and len(nmea_parts) >= 6:
hdg = MagneticHeading()
hdg.header.frame_id = device_frame_id
hdg.header.stamp = ros_now
hdg.heading = cast_float(nmea_parts[1])
hdg.mag_dev = cast_float(nmea_parts[2])
hdg.mag_dev_dir = nmea_parts[3]
hdg.mag_var = cast_float(nmea_parts[4])
hdg.mag_var_dir = nmea_parts[5].split('*')[0]
quat = quaternion_from_euler(0.0, 0.0, cast_float(hdg.heading))
hdg.quaternion.x = quat[0]
hdg.quaternion.y = quat[1]
hdg.quaternion.z = quat[2]
hdg.quaternion.w = quat[3]
mag_heading_pub.publish(hdg)
# NMEA Sentence (published regardless of content)
sentence_msg = Sentence()
sentence_msg.header.frame_id = device_frame_id
sentence_msg.header.stamp = ros_now
sentence_msg.sentence = nmea_in
sentence_pub.publish(sentence_msg)
# Node sleeps for 10 Hz
rate.sleep()
#Import ROS stuff
import rospy
from nmea_msgs.msg import Sentence
#Import socket for getting data from android
import socket
#Server setup for recieving GPS data
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
port = 6000
client_address = ('192.168.43.123', port)
#Init rospy stuff
rospy.init_node("gps_publisher.py", anonymous=True)
pub = rospy.Publisher('/nmea_sentence', Sentence, queue_size=10)
s = Sentence()
try:
sock.connect(client_address)
while True:
data = sock.recv(int(1000000))
if data[0] == 'G':
s.sentence = '$' + data[0:data.find('\r')]
s.header.stamp = rospy.Time.now()
s.header.frame_id = 'gps'
print(data[0:data.find('\r')])
pub.publish(s)
finally:
sock.close()