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