def calclst(longitude=-79.8397, latitude=34.4331, elevation=0.):
"""
Compute the ra,dec (J2000) from Az,El location and time
"""
location = ephem.Observer()
location.lon = angles.d2r(longitude)
location.lat = angles.d2r(latitude)
location.elevation = elevation
now = datetime.datetime.utcnow()
strnow = now.isoformat()
dates = strnow.split('T')
datestr = dates[0] + ' ' + dates[1]
location.date = datestr
lst = location.sidereal_time()
print('LST = %s' % (lst))
return lst
def convolve(self, x, y, z, inweight):
"""
convolve a measurement onto a range of grid coordinates
"""
nwidth = int(4.1 * self.FWHM * self.dpi)
ix = self.ii(x)
jy = self.jj(y)
x0 = angles.d2r(x) # convert to radians
if x0 < self.xminrad:
x0 = x0 + (2. * np.pi)
elif x0 > self.xmaxrad:
x0 = x0 - (2. * np.pi)
y0 = angles.d2r(y)
for iii in range(-nwidth, nwidth):
iix = ix + iii
if iix < 0:
iix = self.img_width + iix
elif iix >= self.img_width:
iix = iix - self.img_width
xx = self.xx(iix)
rx = angles.d2r(xx) # convert to radians
for jjj in range(-nwidth, nwidth):
jjy = jy + jjj
if jjy < 0:
jjy = self.img_height + jjy
elif jjy >= self.img_height:
jjy = jjy - self.img_height
yy = self.yy(jjy)
ry = angles.d2r(yy) # conver to radians
if ry < self.yminrad:
ry = ry + (2. * np.pi)
elif ry > self.ymaxrad:
ry = ry - (2. * np.pi)
# finally get angular separation in degrees
r = angles.r2d(angles.sep(x0, y0, rx, ry))
# if r > 4.*self.FWHM: # round convolving function
if r > 3. * self.FWHM: # round convolving function
continue
# add the colvolved measurement to the grid.
self.xy(iix, jjy, z, r, inweight)
return
def friend(ob1, ob2):
# determines the friendship between two clumps
flag = "NO" # Initial assumption = NO
""" Projected Sky Separation """
# calculates the angular separation between two centroids of the clumps
t1 = (cat['ra'][ob1], cat['dec'][ob1])
t2 = (cat['ra'][ob2], cat['dec'][ob2])
angrad = sep(d2r(t1[0]), d2r(t1[1]), d2r(t2[0]), d2r(t2[1]))
angarcmin = (angrad * 180 * 60) / float(np.pi)
""" Redshift check"""
# checks redshift consistency of the two clumps.
# If their redshift difference is consistent with zero then yes.
# What todo : Old Method
# z1, z2 = gcat[gindex[id1]]['Z'], gcat[gindex[id2]]['Z'] # corresponding member redshifts
# m1, m2 = np.mean(z1), np.mean(z2) # mean redshifts of the clumps
# var1, var2 = (0.061**2) * np.sum((1+z1)**2), (0.061**2) * np.sum((1+z2)**2) # variances
# er1, er2 = np.sqrt(var1 / float(len(z1))), np.sqrt(var2 / float(len(z2))) # errors
id1, id2 = cat['id'][ob1], cat['id'][
ob2] # clump ids (actually dog ids)
res1 = gfit_to_pdf(id1, gindex)
res2 = gfit_to_pdf(id2, gindex)
m1, m2 = abs(res1[0]), abs(res2[0])
er1, er2 = abs(res1[1]), abs(res2[1])
if m1 > m2:
d = m1 - m2 # difference of the mean
else:
d = m2 - m1
d_er = np.sqrt(er1**2 + er2**2) # error on the difference.
low, high = d - d_er, d + d_er # minimum and maximum difference.
""" Decision """
if angarcmin < aperture:
if low <= 0 <= high:
flag = "YES"
# friend if angular separation is less than the aperture radius
# and if their 0 belongs to the range [min_diff, max_diff].
return flag
def main():
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_quadrotor', anonymous=True)
q = Quadrotor(display=False, debug=True)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
p = q.get_current_pose()
p.x += random.uniform(-2.5, 2.5)
p.y += random.uniform(-2.5, 2.5)
p.z += random.uniform(-2.5, 2.5)
p.yaw += random.uniform(angles.d2r(-90.0), angles.d2r(90.0))
p.z = max(p.z, 0.5)
q.moveto_pose(p, wait=True)
rate.sleep()
moveit_commander.roscpp_shutdown()
def test_angle_class_must_handle_assignments():
a = Angle(d=10.5)
a.r = d2r(45.0)
v = 45.0
assert a.r == d2r(v)
assert a.h == d2h(v)
assert a.arcs == d2arcs(v)
assert a.ounit == 'degrees'
v = 46.0
# assignment
a.r = d2r(v)
assert a.r == d2r(v)
assert a.h == d2h(v)
assert a.d == v
assert a.arcs == d2arcs(v)
assert a.ounit == 'degrees' # no change
v = 49.0
a.d = v
assert a.r == d2r(v)
assert a.h == d2h(v)
assert a.d == v
assert a.arcs == d2arcs(v)
assert a.ounit == 'degrees' # no change
v = 10
a.h = v
assert a.r == h2r(v)
assert a.h == v
assert a.d == h2d(v)
assert a.arcs == h2arcs(v)
assert a.ounit == 'degrees' # no change
v = 3600.0
a.arcs = v
assert a.r == arcs2r(v)
assert a.h == arcs2h(v)
assert a.d == arcs2d(v)
assert a.arcs == v
assert a.ounit == 'degrees' # no change
def test_angle_class_must_handle_assignments():
a = Angle(d=10.5)
a.r = d2r(45.0)
v = 45.0
assert a.r == d2r(v)
assert a.h == d2h(v)
assert a.arcs == d2arcs(v)
assert a.ounit == 'degrees'
v = 46.0
# assignment
a.r = d2r(v)
assert a.r == d2r(v)
assert a.h == d2h(v)
assert a.d == v
assert a.arcs == d2arcs(v)
assert a.ounit == 'degrees' # no change
v = 49.0
a.d = v
assert a.r == d2r(v)
assert a.h == d2h(v)
assert a.d == v
assert a.arcs == d2arcs(v)
assert a.ounit == 'degrees' # no change
v = 10
a.h = v
assert a.r == h2r(v)
assert a.h == v
assert a.d == h2d(v)
assert a.arcs == h2arcs(v)
assert a.ounit == 'degrees' # no change
v = 3600.0
a.arcs = v
assert a.r == arcs2r(v)
assert a.h == arcs2h(v)
assert a.d == arcs2d(v)
assert a.arcs == v
assert a.ounit == 'degrees' # no change
def calcazel(ra, dec, longitude=-79.8397, latitude=38.4331, elevation=800.):
"""
Compute the ra,dec (J2000) from Az,El location and time
"""
location = ephem.Observer()
location.lon = angles.d2r(longitude)
location.lat = angles.d2r(latitude)
location.elevation = elevation
now = datetime.datetime.utcnow()
strnow = now.isoformat()
dates = strnow.split('T')
datestr = dates[0] + ' ' + dates[1]
location.date = datestr
lst = location.sidereal_time()
fmt = 'Date = %s, LST = %s'
print fmt % (datestr, lst)
# first put ra,dec in ephemeris format
radec2000 = ephem.Equatorial( ra, dec, epoch=ephem.J2000)
# now compute ra, dec of date
radec = ephem.Equatorial(radec2000.ra, radec2000.dec, epoch=datestr)
print 'Ra,Dec = %s, %s (J2000)' % (radec2000.ra, radec2000.dec)
gal = ephem.Galactic(radec2000)
print 'Lon,Lat= %s, %s (Galactic)' % (gal.lon, gal.lat)
object = ephem.FixedBody()
object._ra = radec.ra
object._dec = radec.dec
object.compute(location)
# need to turn dd:mm:ss into float
aparts = angles.phmsdms(str(object.az))
az = angles.sexa2deci(aparts['sign'], *aparts['vals'], todeg=False)
aparts = angles.phmsdms(str(object.alt))
el = angles.sexa2deci(aparts['sign'], *aparts['vals'], todeg=False)
# print "Az: %s -> %f" % (str(object.az), az)
# print "El: %s -> %f" % (str(object.alt), el)
fmt = 'Az, El = %.2f, %.2f (degrees)'
print fmt % (az, el)
return az, el
def loop(r_f, z_f, yaw_f):
command = addict.Dict()
traj = []
for d in range(0, 361, 1):
theta = angles.d2r(d)
r = r_f(theta)
z = z_f(r, theta)
yaw = yaw_f(r, theta)
x = r * math.cos(theta)
y = r * math.sin(theta)
traj.append(np.r_[x, y, z, 0.0, 0.0, yaw])
command.trajectory = [addict.Dict(x=t[0], y=t[1], z=t[2], yaw=t[5]) for t in traj]
return command
def stanley_control(self, cx, cy, cyaw):
cyaw = cyaw + d2r(90)
closest_point = self.closest_node([cx, cy], self._waypoints)
for i in range(len(self._waypoints)):
if self._waypoints[i][0] == closest_point[0] and self._waypoints[
i][1] == closest_point[1] and i not in (range(0, 10)):
desired_yaw = np.arctan2(
closest_point[1] - self._waypoints[i - 10][1],
closest_point[0] - self._waypoints[i - 10][0]) + d2r(90)
cross_track_error = self.cross_track_error(
cx, cy, self._waypoints[i], self._waypoints[i - 10])
crosstrack_error_dot = k * cross_track_error # e_dot
cross_track_steering = np.arctan2(
crosstrack_error_dot,
Ks + self._current_speed) # corrects cross track error
# normalized_desired_yaw = self.normalize_angle(desired_yaw)
normalized_desired_yaw = desired_yaw
break
heading_error = normalized_desired_yaw - cyaw # corrects the heading error
delta = heading_error + cross_track_steering
normalized_delta = (1.22 if delta >= 1.22 else
(-1.22 if delta <= -1.22 else delta))
# self.log_stanley(cross_track_error=cross_track_error, cross_track_steering=cross_track_steering,
# heading_error=heading_error,
# desired_yaw=desired_yaw,
# current_yaw=cyaw,
# delta=delta,
# normalized_delta=normalized_delta)
# self.end_logging()
return normalized_delta
def loop(r_f, z_f, yaw_f):
command = addict.Dict()
traj = []
for d in range(0, 361, 1):
theta = angles.d2r(d)
r = r_f(theta)
z = z_f(r, theta)
yaw = yaw_f(r, theta)
x = r * math.cos(theta)
y = r * math.sin(theta)
traj.append(np.r_[x, y, z, 0.0, 0.0, yaw])
command.trajectory = [
addict.Dict(x=t[0], y=t[1], z=t[2], yaw=t[5]) for t in traj
]
return command
def normalize_angles(cls, radian_vector):
return np.asarray(map(lambda x: angles.d2r(angles.normalize(angles.r2d(x), -180, +180)), radian_vector))
def test_cartesian_vector_normalize_sphere():
a = (180, 91)
r = (0, 89)
v = CartesianVector.from_spherical(r=1.0, alpha=d2r(a[0]), delta=d2r(a[1]))
x = [r2d(i) for i in v.normalized_angles]
assert (round(x[0], 12), round(x[1], 12)) == r
a = (180, -91)
r = (0, -89)
v = CartesianVector.from_spherical(r=1.0, alpha=d2r(a[0]), delta=d2r(a[1]))
x = [r2d(i) for i in v.normalized_angles]
assert (round(x[0], 12), round(x[1], 12)) == r
a = (0, 91)
r = (180, 89)
v = CartesianVector.from_spherical(r=1.0, alpha=d2r(a[0]), delta=d2r(a[1]))
x = [r2d(i) for i in v.normalized_angles]
assert (round(x[0], 12), round(x[1], 12)) == r
a = (0, -91)
r = (180, -89)
v = CartesianVector.from_spherical(r=1.0, alpha=d2r(a[0]), delta=d2r(a[1]))
x = [r2d(i) for i in v.normalized_angles]
assert (round(x[0], 12), round(x[1], 12)) == r
a = (120, 280)
r = (120, -80)
v = CartesianVector.from_spherical(r=1.0, alpha=d2r(a[0]), delta=d2r(a[1]))
x = [r2d(i) for i in v.normalized_angles]
assert (round(x[0], 12), round(x[1], 12)) == r
a = (375, 45) # 25 hours, 45 degrees
r = (15, 45)
v = CartesianVector.from_spherical(r=1.0, alpha=d2r(a[0]), delta=d2r(a[1]))
x = [r2d(i) for i in v.normalized_angles]
assert (round(x[0], 12), round(x[1], 12)) == r
a = (-375, -45)
r = (345, -45)
v = CartesianVector.from_spherical(r=1.0, alpha=d2r(a[0]), delta=d2r(a[1]))
x = [r2d(i) for i in v.normalized_angles]
assert (round(x[0], 12), round(x[1], 12)) == r
a = (-375, -91)
r = (165, -89)
v = CartesianVector.from_spherical(r=1.0, alpha=d2r(a[0]), delta=d2r(a[1]))
x = [r2d(i) for i in v.normalized_angles]
assert (round(x[0], 12), round(x[1], 12)) == r
def normalizeRad(self, value): deg = angles.r2d(float(value)) return angles.d2r(angles.normalize(deg, -180, 180))
def test_angular_position_bear():
a = AngularPosition(45.0, 45.0)
b = AngularPosition(45.0, -45.0)
assert round(a.bear(b), 12) == round(d2r(180), 12)
def test_angular_position_sep():
a = AngularPosition(45.0, 45.0)
b = AngularPosition(45.0, -45.0)
assert round(a.sep(b), 12) == round(d2r(90), 12)
def test_cartesian_vector_normalize_sphere():
a = (180, 91)
r = (0, 89)
v = CartesianVector.from_spherical(r=1.0, alpha=d2r(a[0]), delta=d2r(a[1]))
x = [r2d(i) for i in v.normalized_angles]
assert (round(x[0], 12), round(x[1], 12)) == r
a = (180, -91)
r = (0, -89)
v = CartesianVector.from_spherical(r=1.0, alpha=d2r(a[0]), delta=d2r(a[1]))
x = [r2d(i) for i in v.normalized_angles]
assert (round(x[0], 12), round(x[1], 12)) == r
a = (0, 91)
r = (180, 89)
v = CartesianVector.from_spherical(r=1.0, alpha=d2r(a[0]), delta=d2r(a[1]))
x = [r2d(i) for i in v.normalized_angles]
assert (round(x[0], 12), round(x[1], 12)) == r
a = (0, -91)
r = (180, -89)
v = CartesianVector.from_spherical(r=1.0, alpha=d2r(a[0]), delta=d2r(a[1]))
x = [r2d(i) for i in v.normalized_angles]
assert (round(x[0], 12), round(x[1], 12)) == r
a = (120, 280)
r = (120, -80)
v = CartesianVector.from_spherical(r=1.0, alpha=d2r(a[0]), delta=d2r(a[1]))
x = [r2d(i) for i in v.normalized_angles]
assert (round(x[0], 12), round(x[1], 12)) == r
a = (375, 45) # 25 hours, 45 degrees
r = (15, 45)
v = CartesianVector.from_spherical(r=1.0, alpha=d2r(a[0]), delta=d2r(a[1]))
x = [r2d(i) for i in v.normalized_angles]
assert (round(x[0], 12), round(x[1], 12)) == r
a = (-375, -45)
r = (345, -45)
v = CartesianVector.from_spherical(r=1.0, alpha=d2r(a[0]), delta=d2r(a[1]))
x = [r2d(i) for i in v.normalized_angles]
assert (round(x[0], 12), round(x[1], 12)) == r
a = (-375, -91)
r = (165, -89)
v = CartesianVector.from_spherical(r=1.0, alpha=d2r(a[0]), delta=d2r(a[1]))
x = [r2d(i) for i in v.normalized_angles]
assert (round(x[0], 12), round(x[1], 12)) == r
def sep_tuple(t1,
t2): # distances between two coordinates, stored in two tuples
# works fine, but will not use
angrad = sep(d2r(t1[0]), d2r(t1[1]), d2r(t2[0]), d2r(t2[1]))
angarcmin = (angrad * 180 * 60) / float(np.pi) # in arcmin
return angarcmin
def test_angular_position_sep():
a = AngularPosition(45.0, 45.0)
b = AngularPosition(45.0, -45.0)
assert round(a.sep(b), 12) == round(d2r(90), 12)
#import radec2azel import radioastronomy import copy #from scipy.signal import savgol_filter import interpolate linelist = [1420.0, 1419.0, 1418.0] # RFI lines in MHz linewidth = [9, 5, 7] #for 2017-07-21 observations eloffset = -4.91 azoffset = -18.0 #for 2017-09-03 observations eloffset = 0.52 azoffset = -15.36 fwhm = 16.0 # telescope fwhm angle degrees onTheta = angles.d2r(0.4*fwhm) offThetaA = angles.d2r(0.5*fwhm) offThetaB = angles.d2r(1.0*fwhm) nargs = len(sys.argv) linestyles = ['-','-','-', '-.','-.','-.','--','--','--','-','-','-', '-.','-.','-.','--','--','--','-','-','-', '-.','-.','-.','--','--','--','-','-','-', '-.','-.','-.','--','--','--'] colors = ['-b','-r','-g', '-b','-r','-g','-b','-r','-g','-b','-r','-g','-b','-r','-g','-b','-r','-g','-b','-r','-g','-b','-r','-g','-b','-r','-g','-b','-r','-g','-b','-r','-g','-b','-r','-g'] nmax = len(colors) scalefactor = 1e8 xallmax = -9.e9 xallmin = 9.e9 yallmax = -9.e9 yallmin = 9.e9 # velocities for fitting baselines minvel = -450.
def test_angular_position_bear():
a = AngularPosition(45.0, 45.0)
b = AngularPosition(45.0, -45.0)
assert round(a.bear(b), 12) == round(d2r(180), 12)