def update(self, event):
if self.last_command_timestamp is None or event.current_real - self.last_command_timestamp > rospy.Duration.from_sec(
0.5 * self.time_factor):
self.throttle = 0.0
self.rudder = 0.0
self.dynamics.update(self.throttle, self.rudder, event.current_real)
gps = GeoPointStamped()
gps.header.stamp = self.dynamics.last_update
gps.position.latitude = math.degrees(self.dynamics.latitude)
gps.position.longitude = math.degrees(self.dynamics.longitude)
self.position_publisher.publish(gps)
ts = TwistStamped()
ts.header.stamp = self.dynamics.last_update
# TODO should this be split in x and y components when heading != course?
ts.twist.linear.x = self.dynamics.sog
self.speed_publisher.publish(ts)
#p.basic_position.cog = self.dynamics.cog
#p.basic_position.sog = self.dynamics.sog
#p.header.stamp = self.dynamics.last_update
h = NavEulerStamped()
h.header.stamp = self.dynamics.last_update
h.orientation.heading = math.degrees(self.dynamics.heading)
self.heading_publisher.publish(h)
def posmv_listener():
position_pub = rospy.Publisher('/posmv/position', NavSatFix, queue_size=10)
timeref_pub = rospy.Publisher('/posmv/time_reference',
TimeReference,
queue_size=10)
orientation_pub = rospy.Publisher('/posmv/orientation',
NavEulerStamped,
queue_size=10)
rospy.init_node('posmv')
pos = posmv.Posmv()
gps_week = None
gps_utc_offset = None
timestamp = datetime.datetime.utcfromtimestamp(
rospy.Time.now().to_time()).isoformat()
bag = rosbag.Bag(
'nodes/posmv_' + ('-'.join(timestamp.split(':'))) + '.bag', 'w',
rosbag.Compression.BZ2)
while not rospy.is_shutdown():
data = pos.read((1, 3))
#print data
for d in data:
if d['group_id'] == 1:
now = rospy.get_rostime()
pos_now = decode_time(d, gps_week, gps_utc_offset)
if pos_now is not None:
tref = TimeReference()
tref.header.stamp = now
tref.time_ref = rospy.Time(
calendar.timegm(pos_now.timetuple()),
pos_now.microsecond * 1000)
tref.source = 'posmv'
timeref_pub.publish(tref)
bag.write('/posmv/time_reference', tref)
nsf = NavSatFix()
nsf.header.stamp = now
nsf.header.frame_id = 'posmv'
nsf.latitude = d['latitude']
nsf.longitude = d['longitude']
nsf.altitude = d['altitude']
position_pub.publish(nsf)
bag.write('/posmv/position', nsf)
nes = NavEulerStamped()
nes.header.stamp = now
nes.header.frame_id = 'posmv'
nes.orientation.roll = d['vessel_roll']
nes.orientation.pitch = d['vessel_pitch']
nes.orientation.heading = d['vessel_heading']
orientation_pub.publish(nes)
bag.write('/posmv/orientation', nes)
if d['group_id'] == 3:
gps_week = d['gps_utc_week_number']
gps_utc_offset = d['gps_utc_time_offset']
def imuCallback(self, data):
#self.heading = data.data+self.magnetic_declination
nes = NavEulerStamped()
nes.header = data.header
orientation_q = data.orientation
orientation_list = [
orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w
]
(roll, pitch, yaw) = euler_from_quaternion(orientation_list)
self.heading = 90 - math.degrees(yaw)
nes.orientation.heading = self.heading
self.heading_pub.publish(nes)
nes.orientation.pitch = math.degrees(pitch)
nes.orientation.roll = math.degrees(roll)
self.posmv_orientation_pub.publish(nes)
def update(self, event):
if self.last_command_timestamp is None or event.current_real - self.last_command_timestamp > rospy.Duration.from_sec(
0.5 * self.time_factor):
self.throttle = 0.0
self.rudder = 0.0
diag = self.dynamics.update(self.throttle, self.rudder,
event.current_real)
gps = GeoPointStamped()
gps.header.stamp = self.dynamics.last_update
gps.position.latitude = math.degrees(self.dynamics.latitude)
gps.position.longitude = math.degrees(self.dynamics.longitude)
self.position_publisher.publish(gps)
ts = TwistStamped()
ts.header.stamp = self.dynamics.last_update
# TODO should this be split in x and y components when heading != course?
ts.twist.linear.x = self.dynamics.sog
self.speed_publisher.publish(ts)
#p.basic_position.cog = self.dynamics.cog
#p.basic_position.sog = self.dynamics.sog
#p.header.stamp = self.dynamics.last_update
h = NavEulerStamped()
h.header.stamp = self.dynamics.last_update
h.orientation.heading = math.degrees(self.dynamics.heading)
self.heading_publisher.publish(h)
for key, value in diag.items():
if not key in self.diag_publishers:
self.diag_publishers[key] = rospy.Publisher(
'/asv_sim/diagnostics/' + key, Float64, queue_size=5)
self.diag_publishers[key].publish(value)
def update(self, event):
if self.last_command_timestamp is None or event.current_real - self.last_command_timestamp > rospy.Duration.from_sec(
0.5 * self.time_factor):
self.throttle = 0.0
self.rudder = 0.0
self.dynamics.update(self.throttle, self.rudder, event.current_real)
gps = GeoPointStamped()
gps.header.stamp = self.dynamics.last_update
gps.position.latitude = math.degrees(self.dynamics.latitude)
gps.position.longitude = math.degrees(self.dynamics.longitude)
self.position_publisher.publish(gps)
#p.basic_position.cog = self.dynamics.cog
#p.basic_position.sog = self.dynamics.sog
#p.header.stamp = self.dynamics.last_update
h = NavEulerStamped()
h.header.stamp = self.dynamics.last_update
h.orientation.heading = math.degrees(self.dynamics.heading)
self.heading_publisher.publish(h)
def posmv_nmea_listener():
position_pub = rospy.Publisher('/base/position', NavSatFix, queue_size=10)
timeref_pub = rospy.Publisher('/base/time_reference',
TimeReference,
queue_size=10)
orientation_pub = rospy.Publisher('/base/orientation',
NavEulerStamped,
queue_size=10)
rospy.init_node('posmv_nmea')
input_type = rospy.get_param('/posmv_nmea/input_type')
input_address = rospy.get_param('/posmv_nmea/input', '')
input_speed = rospy.get_param('/posmv_nmea/input_speed', 0)
input_port = int(rospy.get_param('/posmv_nmea/input_port', 0))
output_port = int(rospy.get_param('/posmv_nmea/output', 0))
output_address = rospy.get_param('/posmv_nmea/output_address',
'<broadcast>')
if input_type == 'serial':
serial_in = serial.Serial(input_address, int(input_speed))
else:
udp_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
udp_in.bind(('', input_port))
if output_port > 0:
udp_out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM,
socket.IPPROTO_UDP)
udp_out.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
else:
udp_out = None
timestamp = datetime.datetime.utcfromtimestamp(
rospy.Time.now().to_time()).isoformat()
bag = rosbag.Bag(
'nodes/posmv_nmea_' + ('-'.join(timestamp.split(':'))) + '.bag', 'w',
rosbag.Compression.BZ2)
while not rospy.is_shutdown():
if input_type == 'serial':
nmea_in = serial_in.readline()
#print nmea_in
if udp_out is not None:
udp_out.sendto(nmea_in, (output_address, output_port))
else:
nmea_in, addr = udp_in.recvfrom(1024)
#print addr, nmea_in
now = rospy.get_rostime()
nmea_parts = nmea_in.strip().split(',')
if len(nmea_parts):
#print nmea_parts
try:
if nmea_parts[0] == '$GPZDA' and len(nmea_parts) >= 5:
tref = TimeReference()
tref.header.stamp = now
hour = int(nmea_parts[1][0:2])
minute = int(nmea_parts[1][2:4])
second = int(nmea_parts[1][4:6])
ms = int(float(nmea_parts[1][6:]) * 1000000)
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 = 'posmv'
timeref_pub.publish(tref)
bag.write('/posmv_nmea/time_reference', tref)
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 = now
nsf.header.frame_id = 'posmv_operator'
nsf.latitude = latitude
nsf.longitude = longitude
nsf.altitude = altitude
position_pub.publish(nsf)
bag.write('/posmv_nmea/position', nsf)
if nmea_parts[0] == '$GPHDT' and len(nmea_parts) >= 2:
heading = float(nmea_parts[1])
nes = NavEulerStamped()
nes.header.stamp = now
nes.header.frame_id = 'posmv_operator'
nes.orientation.heading = heading
orientation_pub.publish(nes)
bag.write('/posmv_nmea/orientation', nes)
except ValueError:
pass
bag.close()
def ais_listener(logdir=None):
position_pub = rospy.Publisher('/base/position', NavSatFix, queue_size=10)
timeref_pub = rospy.Publisher('/base/time_reference',
TimeReference,
queue_size=10)
ais_pub = rospy.Publisher('/base/ais/contacts', Contact, queue_size=10)
ais_raw_pub = rospy.Publisher('/base/ais/raw', Heartbeat, queue_size=10)
rospy.init_node('ais')
input_type = rospy.get_param('/ais/input_type')
input_address = rospy.get_param('/ais/input', '')
input_speed = rospy.get_param('/ais/input_speed', 0)
input_port = int(rospy.get_param('/ais/input_port', 0))
output_port = int(rospy.get_param('/ais/output', 0))
output_address = rospy.get_param('/ais/output_address', '<broadcast>')
ais_decoder = ais.decoder.AISDecoder()
if logdir is not None:
logfile = file(
logdir +
'.'.join(datetime.datetime.utcnow().isoformat().split(':')) +
'_ais.log', 'w')
else:
logfile = None
if input_type == 'serial':
serial_in = serial.Serial(input_address, int(input_speed))
else:
udp_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
udp_in.bind(('', input_port))
if output_port > 0:
udp_out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM,
socket.IPPROTO_UDP)
udp_out.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
else:
udp_out = None
while not rospy.is_shutdown():
if input_type == 'serial':
nmea_ins = (serial_in.readline(), )
#print nmea_in
if udp_out is not None:
udp_out.sendto(nmea_in, (output_address, output_port))
else:
nmea_in, addr = udp_in.recvfrom(2048)
#print addr, nmea_in
nmea_ins = nmea_in.split('\n')
now = rospy.get_rostime()
for nmea_in in nmea_ins:
if logfile is not None:
logfile.write(datetime.datetime.utcnow().isoformat() + ',' +
nmea_in + '\n')
if nmea_in.startswith('!AIVDM'):
ais_decoder.addNMEA(nmea_in.strip())
msgs = ais_decoder.popMessages()
for m in msgs:
if m['type'] in (1, 2, 3, 18, 19): #position reports
c = Contact()
c.header.stamp = now
c.mmsi = m['mmsi']
if 'shipname' in ais_decoder.mmsi_db[m['mmsi']]:
c.name = ais_decoder.mmsi_db[m['mmsi']]['shipname']
if 'callsign' in ais_decoder.mmsi_db[m['mmsi']]:
c.callsign = ais_decoder.mmsi_db[
m['mmsi']]['callsign']
c.position.latitude = m['lat']
c.position.longitude = m['lon']
if m['course'] is not None:
c.cog = math.radians(m['course'])
else:
c.cog = -1
if m['speed'] is not None:
c.sog = m['speed'] * 0.514444 #knots to m/s
else:
c.sog = -1
if m['heading'] is not None:
c.heading = math.radians(m['heading'])
else:
c.heading = -1
if 'to_bow' in ais_decoder.mmsi_db[m['mmsi']]:
c.dimension_to_bow = ais_decoder.mmsi_db[
m['mmsi']]['to_bow']
if 'to_stern' in ais_decoder.mmsi_db[m['mmsi']]:
c.dimension_to_stern = ais_decoder.mmsi_db[
m['mmsi']]['to_stern']
if 'to_port' in ais_decoder.mmsi_db[m['mmsi']]:
c.dimension_to_port = ais_decoder.mmsi_db[
m['mmsi']]['to_port']
if 'to_starboard' in ais_decoder.mmsi_db[m['mmsi']]:
c.dimension_to_stbd = ais_decoder.mmsi_db[
m['mmsi']]['to_starboard']
ais_pub.publish(c)
raw = Heartbeat()
for k, v in m.iteritems():
raw.values.append(KeyValue(k, str(v)))
ais_raw_pub.publish(raw)
else:
nmea_parts = nmea_in.strip().split(',')
if len(nmea_parts):
#print nmea_parts
try:
if nmea_parts[0][3:] == 'ZDA' and len(nmea_parts) >= 5:
tref = TimeReference()
tref.header.stamp = now
hour = int(nmea_parts[1][0:2])
minute = int(nmea_parts[1][2:4])
second = int(nmea_parts[1][4:6])
ms = int(float(nmea_parts[1][6:]) * 1000000)
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 = 'ais'
timeref_pub.publish(tref)
if nmea_parts[0][3:] == 'GGA' 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 = now
nsf.header.frame_id = 'mobile_lab'
nsf.latitude = latitude
nsf.longitude = longitude
nsf.altitude = altitude
position_pub.publish(nsf)
if nmea_parts[0][3:] == 'HDT' and len(nmea_parts) >= 2:
heading = float(nmea_parts[1])
nes = NavEulerStamped()
nes.header.stamp = now
nes.header.frame_id = 'mobile_lab'
nes.orientation.heading = heading
orientation_pub.publish(nes)
except ValueError:
pass
def headingCallback(self, data):
self.heading = data.data + self.magnetic_declination
nes = NavEulerStamped()
nes.header.stamp = rospy.get_rostime()
nes.orientation.heading = self.heading
self.heading_pub.publish(nes)
def run_test(self, filename):
lines = []
index = 0
obstacles = []
map_file = ""
start = []
self.stats["time_limit"] = self.default_time_limit
parameter_file_names = []
try:
with open(filename, "r") as testfile:
for line in testfile:
line = line.strip()
if line.startswith("line"):
lines.append([float(f) for f in line.split(" ")[1:]])
print("Read line ", lines[-1])
assert len(lines[-1]
) == 4 # should be startX startY endX endY
elif line.startswith("start"):
start = [float(f) for f in line.split(" ")[1:]]
start[2] = math.radians(start[2])
# assert len(start) == 4 # x y heading speed # assume speed is zero?
elif line.startswith("obstacle"):
obs = [float(f) for f in line.split(" ")[1:]]
if len(obs) == 4:
obs.append(5)
obs.append(20)
obstacles.append(obs)
# ignore test-defined time limit (deprecated)
# elif line.startswith("time_limit"):
# self.stats["time_limit"] = float(line[10:])
elif line.startswith("map_file"):
map_file = line[8:].strip()
elif line.startswith("parameter_file"):
parameter_file_names.append(line[15:])
except IOError as err:
print("Couldn't find file: " + filename)
return
try:
# Convert to lat long
lines = [self.convert_line(line) for line in lines]
except rospy.ServiceException as exc:
print("Map to LatLong service did not process request: " +
str(exc))
# # wait until clock is initialized
# while rospy.get_time() == 0:
# pass
if rospy.get_time() == 0:
print("Simulation does not appear to be running yet. Exiting.")
return
# load parameters and update dynamic reconfigure
self.load_parameters(parameter_file_names)
# load map file, if any
if map_file:
current_path = os.getcwd()
self.path_planner_parameter_client.update_configuration(
{"planner_geotiff_map": current_path + "/" + map_file})
else:
self.path_planner_parameter_client.update_configuration(
{"planner_geotiff_map": ""})
self.test_name = filename
# create results directory
now = datetime.now()
results_dir_path = "../results/" + now.strftime("%Y_%m_%d/%H:%M:%S_" +
self.test_name)
if not os.path.exists(results_dir_path):
os.makedirs(results_dir_path)
# initialize bag
self.bag_lock.acquire()
self.bag = rosbag.Bag(results_dir_path + "/logs.bag", 'w')
self.bag_lock.release()
self.piloting_mode_publisher.publish("autonomous")
# Set up set_pose request
if start:
gps = GeoPointStamped()
gps.position = self.convert_point(start[0], start[1])
h = NavEulerStamped()
h.orientation.heading = start[2]
self.set_pose(gps, h)
else:
print("Warning: no start state read; using default start state")
self.reset_publisher.publish(True)
rospy.sleep(0.2) # Let simulator reset
# finish setting up obstacles
for o in obstacles:
o.append(rospy.get_time())
o[2] = math.radians(o[2])
self.start_time = rospy.get_time()
self.end_time = self.start_time + self.stats["time_limit"]
# set up both path_planner goal and display for lines
goal = path_planner.msg.path_plannerGoal()
goal.path.header.stamp = rospy.Time.now()
display_item = GeoVizItem()
display_item.id = "current_path"
for line in lines:
# now sending all lines at once, so points are to be read in pairs
display_points = GeoVizPointList()
display_points.color.r = 1.0
display_points.color.a = 1.0
display_points.size = 5.0
for p in line:
pose = GeoPoseStamped()
pose.pose.position = p
goal.path.poses.append(pose)
display_points.points.append(p)
# TODO! -- goal.speed?
display_item.lines.append(display_points)
self.display_publisher.publish(display_item)
self.test_running = True
self.path_planner_client.wait_for_server()
self.path_planner_client.send_goal(goal, self.done_callback,
self.active_callback,
self.feedback_callback)
# Spin and publish obstacle updates every 0.5s
self.spin_until_done(obstacles)
print("Test %s complete in %s seconds." %
(self.test_name, (rospy.get_time() - self.start_time)))
# remove line from display
if display_item:
display_item.lines = []
self.display_publisher.publish(display_item)
self.piloting_mode_publisher.publish("standby")
# reporting
self.write_results(results_dir_path)
self.reset_stats()
self.bag_lock.acquire()
self.bag.close()
self.bag = None
self.bag_lock.release()