def create_list(self):
msg = List_coord_msg()
msg.from_node = node_name
while not rospy.is_shutdown():
command = self.command
self.command = ""
if command:
num_x = 0
num_y = 0
try:
coord = command.coord.replace("horizontal", "altaz")
except:
coord = command.coord
try:
offcoord = command.offcoord.replace("horizontal", "altaz")
except:
offcoord = command.offcoord
coord = SkyCoord(command.x,
command.y,
frame=coord,
unit="deg",
location=nanten2,
obstime=dt.utcnow())
_coord = coord.transform_to(offcoord)
for i in range(3601):
if -360. * 3600. <= command.off_x * i <= 360. * 3600.:
num_x = i
pass
else:
break
if -90 * 3600. < _coord.data.lat.arcsec + command.off_y * i < 90 * 3600.:
num_y = i
pass
else:
break
delta_t = min(num_x, num_y)
rospy.loginfo(command.off_x)
print(delta_t)
off_x_list = [
command.off_x * delta_t / 4 * i for i in range(5)
]
off_y_list = [
command.off_y * delta_t / 4 * i for i in range(5)
]
time_list = [
dt.fromtimestamp(command.timestamp + delta_t / 4 * i)
for i in range(5)
]
rospy.loginfo(off_x_list)
ret = calc_offset.calc_offset([command.x for i in range(5)],
[command.y for i in range(5)],
command.coord, off_x_list,
off_y_list, command.offcoord,
command.dcos, time_list)
rospy.loginfo(command.x)
rospy.loginfo(ret[0])
if not ret:
continue
current_time = time.time()
print(ret[0], ret[1])
msg.x_list = ret[0]
msg.y_list = ret[1]
msg.time_list = [
command.timestamp + delta_t / 4 * i for i in range(5)
]
msg.coord = command.coord
msg.off_az = 0
msg.off_el = 0
msg.hosei = command.hosei
msg.lamda = command.lamda
msg.limit = command.limit
msg.timestamp = current_time
self.pub.publish(msg)
self.move_on = command.timestamp + delta_t + 0.1
print(msg)
print("publish status!!\n")
print("end_create_list\n")
else:
if 0 < self.move_on < time.time():
self.pub_stop.publish(True, node_name, time.time())
print("emergency stop!!")
self.move_on = 0
pass
else:
pass
pass
time.sleep(0.1)
return
def create_azel_list(self):
msg = List_coord_msg()
print("wait comming list...")
while (self.param =="") and (not rospy.is_shutdown()) :
time.sleep(0.1)
print("start_calclation!!")
loop = 0
check = 0
param = self.param
while not rospy.is_shutdown():
if not self.param:
time.sleep(1.)
continue
elif param != self.param:
loop = 0
check = 0
param = self.param
else:
pass
if self.stop_flag == False:
if len(param.x_list) > 2:
dt = 0.1
# linear fitting
len_x = param.x_list[loop+1] - param.x_list[loop]
len_y = param.y_list[loop+1] - param.y_list[loop]
len_t = param.time_list[loop+1] - param.time_list[loop]
dx = len_x/(len_t*10)#[arcsec/100ms]
dy = len_y/(len_t*10)#[arcsec/100ms]
dt = 0.1
x_list2 = [param.x_list[loop] + dx*(i+check*100) for i in range(100)]
y_list2 = [param.y_list[loop] + dy*(i+check*100) for i in range(100)]
time_list2 = [param.time_list[loop]+dt*(i+check*100) for i in range(100)]
loop_count = 0
check_count = 1
for i in range(100):
if param.time_list[-1]< time_list2[-1]:
del x_list2[-1]
del y_list2[-1]
del time_list2[-1]
self.stop_flag = True
elif param.time_list[loop+1] <= time_list2[-1]:
del x_list2[-1]
del y_list2[-1]
del time_list2[-1]
loop_count = 1
check = 0
check_count = 0
else:
break
loop += loop_count
if loop == len(param.time_list)-1:
self.stop_flag = True
check += check_count
""" debug
plt.plot(param.time_list, param.x_list,label="target",linestyle='None',marker='.')
plt.plot(time_list2, x_list2,label="v2",linestyle='None',marker='.')
plt.show()
"""
else:
len_x = param.x_list[1] - param.x_list[0]
len_y = param.y_list[1] - param.y_list[0]
len_t = param.time_list[1] - param.time_list[0]
dx = len_x/(len_t*10)#[arcsec/100ms]
dy = len_y/(len_t*10)#[arcsec/100ms]
dt = 0.1
x_list2 = [param.x_list[0] + dx*(i+loop*100) for i in range(100)]
y_list2 = [param.y_list[0] + dy*(i+loop*100) for i in range(100)]
time_list2 = [param.time_list[0]+dt*(i+loop*100) for i in range(100)]
loop += 1
for i in range(100):
if param.time_list[-1]< time_list2[-1]:
del x_list2[-1]
del y_list2[-1]
del time_list2[-1]
self.stop_flag = True
else:
break
if time_list2 != []:
time_list3 = [datetime.fromtimestamp(time_list2[i]) for i in range(len(time_list2))]
astro_time = Time(time_list3)
ret = self.calc.coordinate_calc(x_list2, y_list2, astro_time,
param.coord, param.off_az, param.off_el,
param.hosei, param.lamda, self.weather.press,
self.weather.out_temp, self.weather.out_humi, param.limit)
ret[0] = self.negative_change(ret[0])
else:
limit_flag = True
"""limit check"""
for i in range(len(time_list2)):
if not -240*3600<ret[0][i]<240*3600 or not 0.<=ret[1][i]<80*3600.:
print("reaching soft limit : ", )
print("az : ", ret[0][i]/3600., "[deg]")
print("el : ", ret[1][i]/3600., "[deg]")
self.stop_flag = True
limit_flag = True
data = "reaching soft limit : (az, el)=("+str(ret[0][i]/3600.)+", "+str(ret[1][i]/3600.)+") [deg]"
self.obs_stop.publish(data = data, from_node=node_name, timestamp=time.time())
break
else:
pass
limit_flag = False
if not limit_flag:
self.msg.timestamp = time.time()
self.msg.data = False
self.stop.publish(self.msg)
msg.x_list = ret[0]
msg.y_list = ret[1]
msg.time_list = time_list2
msg.from_node =node_name
msg.timestamp = time.time()
self.pub.publish(msg)
#print("msg", msg)
if param.from_node != "worldcoordinate_otf":
print("#### accele!! ####")
self.pub_accele.publish(self.accele,node_name,time.time())
else:
pass
#print("publish ok")
else:
limit_flag = False
else:
print("###########################")
if param.from_node == "worldcoordinate_otf":
print("#### accele!! ####")
self.pub_accele.publish(self.accele,node_name,time.time())
else:
pass
loop = 0
self.param = ""
pass
time.sleep(0.1)
return
def create_list(self):
msg = List_coord_msg()
msg.from_node = node_name
nanten2 = EarthLocation(lat=-22.96995611 * u.deg,
lon=-67.70308139 * u.deg,
height=4863.85 * u.m)
while not rospy.is_shutdown():
command = self.command
self.command = ""
if command:
print("start_create_list")
#ret = calc_offset.calc_offset(command.x, command.y, command.coord,
# command.off_x, command.off_y, command.offcoord,
# command.dcos)
start_x = command.off_x - float(command.dx) / 2. - float(
command.dx) / float(command.dt) * command.rampt
start_y = command.off_y - float(command.dy) / 2. - float(
command.dy) / float(command.dt) * command.rampt
total_t = command.rampt + command.dt * command.num
end_x = command.off_x + command.dx * (command.num - 0.5)
end_y = command.off_y + command.dy * (command.num - 0.5)
print(start_x, end_x, command.x)
if not command.planet.lower() in self.planet_list:
self.pub_obs_stop.publish("planet name is false...",
node_name, time.time())
rospy.logerr("planet name is false...")
continue
else:
pass
time_list = [
command.timestamp + command.delay,
command.timestamp + command.delay + total_t
]
target_list = get_body(command.planet.lower(),
Time(time_list)) #gcrs
target_list.location = nanten2
altaz_list = target_list.altaz
if not all((0. < i < 90. for i in altaz_list.alt.deg)):
self.pub_obs_stop.publish("This planet is not rizing...",
node_name, time.time())
rospy.logerr("This planet is not rizing...")
continue
else:
pass
ret_start = calc_offset.calc_offset(altaz_list.az.deg,
altaz_list.alt.deg,
"altaz", start_x, start_y,
"altaz", command.dcos,
time_list)
ret_end = calc_offset.calc_offset(altaz_list.az.deg,
altaz_list.alt.deg, "altaz",
end_x, end_y, "altaz",
command.dcos, time_list)
msg.x_list = [ret_start[0], ret_end[1]]
msg.y_list = [ret_start[0], ret_end[1]]
print("x_list", msg.x_list)
print("y_list", msg.y_list)
current_time = time.time()
msg.time_list = [
command.timestamp + command.delay,
command.timestamp + command.delay + total_t
]
msg.coord = command.coord_sys
msg.off_az = 0
msg.off_el = 0
msg.hosei = command.hosei
msg.lamda = command.lamda
msg.limit = command.limit
msg.timestamp = current_time
self.pub.publish(msg)
print(msg)
print("publish status!!\n")
print("end_create_list\n")
else:
pass
time.sleep(0.1)
return
def create_list(self):
msg = List_coord_msg()
msg.from_node = node_name
nanten2 = EarthLocation(lat = -22.96995611*u.deg, lon = -67.70308139*u.deg, height = 4863.85*u.m)
list_num = 600
delta_t = 1 #[s]
while not rospy.is_shutdown():
command = self.command
self.command = ""
if command:
print("start_create_list")
pass
else:
time.sleep(0.1)
continue
#print(command.planet)
time_list = [dt.fromtimestamp(command.timestamp+delta_t*i) for i in range(list_num)]
#time_list = Time(time_list)
if not command.planet.lower() in self.planet_list:
self.pub_obs_stop.publish("planet name is false...", node_name, time.time())
rospy.logerr("planet name is false...")
continue
else:
pass
target_list = get_body(command.planet.lower(), Time(time_list))#gcrs
#print(target_list)
target_list.location = nanten2
altaz_list = target_list.altaz
if not all((0.<i<90. for i in altaz_list.alt.deg)):
self.pub_obs_stop.publish("This planet is not rizing...", node_name, time.time())
rospy.logerr("This planet is not rizing...")
continue
ret = calc_offset.calc_offset(altaz_list.az.deg, altaz_list.alt.deg,
"altaz",
command.off_x, command.off_y, command.offcoord,
command.dcos, time_list)
current_time = time.time()
msg.x_list = ret[0]
msg.y_list = ret[1]
msg.time_list = [command.timestamp+delta_t*i for i in range(list_num)]
msg.coord = "planet"
msg.off_az = ret[2]
msg.off_el = ret[3]
msg.hosei = command.hosei
msg.lamda = command.lamda
msg.limit = command.limit
msg.timestamp = current_time
self.pub.publish(msg)
print(msg.time_list)
print("publish status!!\n")
print("end_create_list\n")
return
def create_list(self):
msg = List_coord_msg()
msg.from_node = node_name
while not rospy.is_shutdown():
command = self.command
self.command = ""
if command:
print("start_create_list")
ret = calc_offset.calc_offset(
[command.x], [command.y], command.coord, [command.off_x],
[command.off_y], command.offcoord, command.dcos,
[dt.fromtimestamp(command.timestamp)])
if not ret:
continue
current_time = time.time()
msg.x_list = [ret[0], ret[0]]
msg.y_list = [ret[1], ret[1]]
msg.time_list = [command.timestamp, command.timestamp + 3600.]
msg.coord = command.coord
msg.off_az = ret[2]
msg.off_el = ret[3]
msg.hosei = command.hosei
msg.lamda = command.lamda
msg.limit = command.limit
msg.rotation = command.rotation
msg.timestamp = current_time
self.pub.publish(msg)
print(msg)
print("publish status!!\n")
print("end_create_list\n")
else:
pass
time.sleep(0.1)
return
def create_list(self):
msg = List_coord_msg()
msg.from_node = node_name
while not rospy.is_shutdown():
command = self.command
self.command = ""
if command:
print("start_create_list")
#ret = calc_offset.calc_offset(command.x, command.y, command.coord,
# command.off_x, command.off_y, command.offcoord,
# command.dcos)
start_x = command.off_x - float(command.dx) / 2. - float(
command.dx) / float(command.dt) * command.rampt
start_y = command.off_y - float(command.dy) / 2. - float(
command.dy) / float(command.dt) * command.rampt
total_t = command.rampt + command.dt * command.num
end_x = command.off_x + command.dx * (command.num - 0.5)
end_y = command.off_y + command.dy * (command.num - 0.5)
print(start_x, end_x, command.x)
#off_dx_vel = (end_x - start_x) / total_t #(obs_end - obs_start)
#off_dy_vel = (end_y - start_y) / total_t #(obs_end - obs_start)
#x_list = [command.x+(start_x+off_dx_vel*i*0.1)/3600. for i in range(int(round(total_t/command.dt))*10)]
#y_list = [command.y+(start_y+off_dy_vel*i*0.1)/3600. for i in range(int(round(total_t/command.dt))*10)]
msg.x_list = [
command.x * 3600. + start_x, command.x * 3600. + end_x
]
msg.y_list = [
command.y * 3600. + start_y, command.y * 3600. + end_y
]
current_time = time.time()
msg.time_list = [
command.timestamp + command.delay,
command.timestamp + command.delay + total_t
]
msg.coord = command.coord_sys
msg.off_az = 0
msg.off_el = 0
msg.hosei = command.hosei
msg.lamda = command.lamda
msg.limit = command.limit
msg.timestamp = current_time
self.pub.publish(msg)
print(msg)
print("publish status!!\n")
print("end_create_list\n")
else:
pass
time.sleep(0.1)
return
def create_azel_list(self):
msg = List_coord_msg()
print("wait comming list...")
while (self.param == "") and (not rospy.is_shutdown()):
time.sleep(0.1)
print("start_calclation!!")
loop = 0
check = 0
param = self.param
while not rospy.is_shutdown():
if not self.param:
time.sleep(1.)
continue
elif param != self.param:
loop = 0
check = 0
param = self.param
else:
pass
if self.stop_flag == False: # and self.move_flag == True:
if len(param.x_list) > 2:
dt = 0.1
'''
# curve fitting
temp_time_list = [i-param.time_list[0] for i in param.time_list]
self.p0 = param.x_list[0]
curve_x, cov_x = curve_fit(self.curve2_fit, temp_time_list, param.x_list)
x_list2 = [self.curve2_fit(dt*(i+loop*10), *curve_x) for i in range(10)]
self.p0 = param.y_list[0]
curve_y, cov_y = curve_fit(self.curve2_fit, temp_time_list, param.y_list)
y_list2 = [self.curve2_fit(dt*(i+loop*10), *curve_y) for i in range(10)]
time_list2 = [param.time_list[0]+dt*(i+loop*10) for i in range(10)]
'''
# linear fitting
len_x = param.x_list[loop + 1] - param.x_list[loop]
len_y = param.y_list[loop + 1] - param.y_list[loop]
len_t = param.time_list[loop + 1] - param.time_list[loop]
dx = len_x / (len_t * 10) #[arcsec/100ms]
dy = len_y / (len_t * 10) #[arcsec/100ms]
dt = 0.1
x_list2 = [
param.x_list[loop] + dx * (i + check * 10)
for i in range(10)
]
y_list2 = [
param.y_list[loop] + dy * (i + check * 10)
for i in range(10)
]
time_list2 = [
param.time_list[loop] + dt * (i + check * 10)
for i in range(10)
]
loop_count = 0
check_count = 1
for i in range(10):
if param.time_list[-1] < time_list2[-1]:
del x_list2[-1]
del y_list2[-1]
del time_list2[-1]
self.stop_flag = True
elif param.time_list[loop + 1] <= time_list2[-1]:
del x_list2[-1]
del y_list2[-1]
del time_list2[-1]
loop_count = 1
check = 0
check_count = 0
else:
break
loop += loop_count
if loop == len(param.time_list) - 1:
self.stop_flag = True
check += check_count
""" debug
plt.plot(param.time_list, param.x_list,label="target",linestyle='None',marker='.')
plt.plot(time_list2, x_list2,label="v2",linestyle='None',marker='.')
plt.show()
"""
else:
len_x = param.x_list[1] - param.x_list[0]
len_y = param.y_list[1] - param.y_list[0]
len_t = param.time_list[1] - param.time_list[0]
dx = len_x / (len_t * 10) #[arcsec/100ms]
dy = len_y / (len_t * 10) #[arcsec/100ms]
dt = 0.1
x_list2 = [
param.x_list[0] + dx * (i + loop * 10)
for i in range(10)
]
y_list2 = [
param.y_list[0] + dy * (i + loop * 10)
for i in range(10)
]
time_list2 = [
param.time_list[0] + dt * (i + loop * 10)
for i in range(10)
]
loop += 1
for i in range(10):
if param.time_list[-1] < time_list2[-1]:
del x_list2[-1]
del y_list2[-1]
del time_list2[-1]
self.stop_flag = True
else:
break
if time_list2 != []:
time_list3 = [
datetime.fromtimestamp(time_list2[i])
for i in range(len(time_list2))
]
astro_time = Time(time_list3)
ret = self.calc.coordinate_calc(
x_list2, y_list2, astro_time, param.coord,
param.off_az, param.off_el, param.hosei, param.lamda,
self.press, self.out_temp, self.out_humi, param.limit,
param.rotation)
if param.rotation:
ret[0] = self.negative_change(ret[0])
else:
limit_flag = True
"""limit check"""
for i in range(len(time_list2)):
if not -240 * 3600 < ret[0][
i] < 240 * 3600 or not 0. <= ret[1][i] < 90 * 3600.:
if param.from_node == "worldcoordinate_linear":
self.stop_flag = True
limit_flag = True
break
print("reaching soft limit : ", )
print("az : ", ret[0][i] / 3600., "[deg]")
print("el : ", ret[1][i] / 3600., "[deg]")
self.stop_flag = True
limit_flag = True
data = "reaching soft limit : (az, el)=(" + str(
ret[0][i] / 3600.) + ", " + str(
ret[1][i] / 3600.) + ") [deg]"
self.obs_stop.publish(data=data,
from_node=node_name,
timestamp=time.time())
break
else:
pass
limit_flag = False
"""
if self.move_count == 0:
#self.msg.timestamp = time.time()
#self.msg.data = self.move_flag
#self.move.publish(self.msg)
rospy.wait_for_service("move_flag")
print("wait comunication to antenna_move...")
response = self.service(True)
if response == False:
limit_flag = True
print("disconnect antenna_move...")
else:
print("connect antenna_move! ")
pass
self.move_count += 1
"""
if not limit_flag:
msg.x_list = ret[0]
msg.y_list = ret[1]
msg.coord = param.coord
msg.time_list = time_list2
msg.from_node = node_name
msg.timestamp = param.timestamp
self.pub.publish(msg)
print("msg", msg)
print("publish ok")
else:
limit_flag = False
move_flag = False
else:
loop = 0
self.param = ""
pass
time.sleep(0.1)
return
def create_azel_list(self):
msg = List_coord_msg()
print("wait comming list...")
while (self.param == "") and (not rospy.is_shutdown()):
time.sleep(0.1)
continue
print("start_calclation!!")
loop = 0
check = 0
param = self.param
while not rospy.is_shutdown():
if not self.param:
time.sleep(1.)
continue
elif param != self.param:
loop = 0
check = 0
param = self.param
else:
pass
if self.stop_flag == False:
if len(param.x_list) > 2:
dt = 0.1
# linear fitting
len_x = param.x_list[loop + 1] - param.x_list[loop]
len_y = param.y_list[loop + 1] - param.y_list[loop]
len_t = param.time_list[loop + 1] - param.time_list[loop]
dx = len_x / (len_t * 10) #[arcsec/100ms]
dy = len_y / (len_t * 10) #[arcsec/100ms]
dt = 0.1
x_list2 = [
param.x_list[loop] + dx * (i + check * 10)
for i in range(10)
]
y_list2 = [
param.y_list[loop] + dy * (i + check * 10)
for i in range(10)
]
time_list2 = [
param.time_list[loop] + dt * (i + check * 10)
for i in range(10)
]
loop_count = 0
check_count = 1
for i in range(10):
if param.time_list[-1] < time_list2[-1]:
del x_list2[-1]
del y_list2[-1]
del time_list2[-1]
self.stop_flag = True
elif param.time_list[loop + 1] <= time_list2[-1]:
del x_list2[-1]
del y_list2[-1]
del time_list2[-1]
loop_count = 1
check = 0
check_count = 0
else:
break
loop += loop_count
if loop == len(param.time_list) - 1:
self.stop_flag = True
check += check_count
else:
len_x = param.x_list[1] - param.x_list[0]
len_y = param.y_list[1] - param.y_list[0]
len_t = param.time_list[1] - param.time_list[0]
dx = len_x / (len_t * 10) #[arcsec/100ms]
dy = len_y / (len_t * 10) #[arcsec/100ms]
dt = 0.1
x_list2 = [
param.x_list[0] + dx * (i + loop * 10)
for i in range(10)
]
y_list2 = [
param.y_list[0] + dy * (i + loop * 10)
for i in range(10)
]
time_list2 = [
param.time_list[0] + dt * (i + loop * 10)
for i in range(10)
]
loop += 1
for i in range(10):
if param.time_list[-1] < time_list2[-1]:
del x_list2[-1]
del y_list2[-1]
del time_list2[-1]
self.stop_flag = True
else:
break
if time_list2 != []:
time_list3 = [
datetime.fromtimestamp(time_list2[i])
for i in range(len(time_list2))
]
astro_time = Time(time_list3)
ret = self.calc.coordinate_calc(
x_list2, y_list2, astro_time, param.coord,
param.off_az, param.off_el, param.hosei, param.lamda,
self.press, self.out_temp, self.out_humi, param.limit,
param.rotation)
if param.rotation:
ret[0] = self.negative_change(ret[0])
else:
limit_flag = True
"""limit check"""
for i in range(len(time_list2)):
if not -240 * 3600 < ret[0][
i] < 240 * 3600 or not 0. <= ret[1][i] < 90 * 3600.:
print("reaching soft limit : ", )
print("az : ", ret[0][i] / 3600., "[deg]")
print("el : ", ret[1][i] / 3600., "[deg]")
self.stop_flag = True
limit_flag = True
break
else:
pass
limit_flag = False
if not limit_flag:
msg.x_list = ret[0]
msg.y_list = ret[1]
msg.coord = param.coord
msg.time_list = time_list2
msg.from_node = node_name
msg.timestamp = time.time()
self.pub.publish(msg)
else:
limit_flag = False
move_flag = False
else:
loop = 0
self.param = ""
pass
time.sleep(0.1)
return